diff --git a/.gitignore b/.gitignore index b3d8edd..ec9bd89 100644 --- a/.gitignore +++ b/.gitignore @@ -63,6 +63,11 @@ Thumbs.db /profiles/ /calibration_output/ /config/*_three_camera_extrinsics.yaml +# Superseded local O6 camera calibrations. Keep the active extrinsics and +# intrinsics referenced by o6_right_product.yaml available for version control. +/config/o6_three_camera_extrinsics_20260915.yaml +/config/o6_three_camera_extrinsics_intrinsics_20260915.yaml +/config/o6_three_camera_extrinsics_recalibrated.yaml *.wear_check.json *.checkpoint.json *.verification.json @@ -73,6 +78,8 @@ Thumbs.db /src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf /src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf /src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf +# Unreferenced local URDF experiment; never use as a protected CAD input. +/src/linkerhand_calibration/urdf/o6_right/linkerhand_o6_right111.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf diff --git a/AGENTS.md b/AGENTS.md index a8e003d..0589e3e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -1 +1,2 @@ -避免产生屎山代码，要求代码结构清晰可读性好 \ No newline at end of file +1.不要补丁式修复问题，避免产生屎山代码，要求代码结构清晰可读性好。 +2.永远用中文回复。 \ No newline at end of file diff --git a/config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml new file mode 100644 index 0000000..8556005 --- /dev/null +++ b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml @@ -0,0 +1,55 @@ +image_width: 1624 +image_height: 1240 +camera_name: hikrobot_top_DB2163739 +camera_matrix: + rows: 3 + cols: 3 + data: + - 3608.7915725635876 + - 0.0 + - 814.529414176388 + - 0.0 + - 3602.9474866035775 + - 596.7866652774017 + - 0.0 + - 0.0 + - 1.0 +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: + - -0.05774549441026762 + - -0.5653814259405426 + - -0.0034831870836035525 + - -0.0007572304719307008 + - 0.0 +rectification_matrix: + rows: 3 + cols: 3 + data: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 +projection_matrix: + rows: 3 + cols: 4 + data: + - 3590.9833984375 + - 0.0 + - 813.6314052451635 + - 0.0 + - 0.0 + - 3591.4892578125 + - 595.0547132430074 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 diff --git a/config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml new file mode 100644 index 0000000..a01efb8 --- /dev/null +++ b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml @@ -0,0 +1,55 @@ +image_width: 1624 +image_height: 1240 +camera_name: hikrobot_front_DB2163742 +camera_matrix: + rows: 3 + cols: 3 + data: + - 3520.83963745858 + - 0.0 + - 971.8024440668337 + - 0.0 + - 3503.4665942352735 + - 636.2092891200001 + - 0.0 + - 0.0 + - 1.0 +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: + - -0.06456607194260039 + - -0.3209716179987898 + - 0.0016533290649511723 + - 0.014394085090833234 + - 0.0 +rectification_matrix: + rows: 3 + cols: 3 + data: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 +projection_matrix: + rows: 3 + cols: 4 + data: + - 3485.41357421875 + - 0.0 + - 980.9585478080553 + - 0.0 + - 0.0 + - 3500.062255859375 + - 636.4474700149367 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 diff --git a/config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml new file mode 100644 index 0000000..79dc878 --- /dev/null +++ b/config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml @@ -0,0 +1,55 @@ +image_width: 1624 +image_height: 1240 +camera_name: hikrobot_side_DB2163749 +camera_matrix: + rows: 3 + cols: 3 + data: + - 3564.4557723541934 + - 0.0 + - 633.5422135270659 + - 0.0 + - 3549.8257661608695 + - 614.1194434338138 + - 0.0 + - 0.0 + - 1.0 +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: + - -0.023159717917942222 + - -0.6611401590594669 + - 0.0010272016326466154 + - -0.014523561130335804 + - 0.0 +rectification_matrix: + rows: 3 + cols: 3 + data: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 +projection_matrix: + rows: 3 + cols: 4 + data: + - 3531.000244140625 + - 0.0 + - 623.4124604711214 + - 0.0 + - 0.0 + - 3551.248291015625 + - 614.0312247691472 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 diff --git a/config/o6_three_camera_extrinsics_20260915_192517.yaml b/config/o6_three_camera_extrinsics_20260915_192517.yaml new file mode 100644 index 0000000..7652e91 --- /dev/null +++ b/config/o6_three_camera_extrinsics_20260915_192517.yaml @@ -0,0 +1,60 @@ +schema_version: 1 +reference_view: front +cameras: + front: + serial_number: DB2163742 + width: 1624 + height: 1240 + intrinsics_sha256: 3cdecb8fcede41561019882a48a342d7265a44855cf0282ec96ec7f69a9b1ee5 + side: + serial_number: DB2163749 + width: 1624 + height: 1240 + intrinsics_sha256: dbe0cdc73460dc1e51c0e3967395d14b4415a71cee952597e0717ad0babd3449 + top: + serial_number: DB2163739 + width: 1624 + height: 1240 + intrinsics_sha256: 315d5cd6f28964ad57ff67088222fff3d9679cce076acc3f05a22b76038e5352 +front_from_view: + front: + translation_xyz_m: + - 0.0 + - 0.0 + - 0.0 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + side: + translation_xyz_m: + - -0.8459732881345877 + - -0.000555404488719817 + - 0.9729938257334647 + quaternion_xyzw: + - -0.04404064417946292 + - 0.6703485002266404 + - -0.01659275028535042 + - 0.7405524900654377 + top: + translation_xyz_m: + - -0.10806018719194868 + - -0.5842672050520756 + - 1.0419844264616385 + quaternion_xyzw: + - 0.7284460604085318 + - -0.016210595429770967 + - 0.019511507802845565 + - -0.6846333724953536 +quality: + passed: true + reprojection_rms_px: 0.46496404895142424 + maximum_rotation_repeatability_deg: 0.019287272875226844 + maximum_translation_repeatability_m: 0.0002246640979668814 + front_side_captures: 15 + front_top_captures: 15 + front_side_candidates: 15 + front_top_candidates: 15 + front_side_rejected: 0 + front_top_rejected: 0 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py index 6e7538d..ab81d74 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py @@ -7,6 +7,7 @@ from enum import Enum from utils.open_can import OpenCan from can.exceptions import CanError from utils.color_msg import ColorMsg +from utils.feedback import ReceivedPositionFrames current_dir = os.path.dirname(os.path.abspath(__file__)) target_dir = os.path.abspath(os.path.join(current_dir, "..")) sys.path.append(target_dir) @@ -178,6 +179,10 @@ class LinkerHandG20Can: # 串联控制数据存储 self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], [] + frame_layout = [[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)] + layout = self.joint_state_to_cmd_state(frame_layout) + self.position_feedback = ReceivedPositionFrames( + {frame: 6 for frame in range(0x41, 0x46)}, [item if item != 0 else None for item in layout]) self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6 self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], [] self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], [] @@ -354,6 +359,8 @@ class LinkerHandG20Can: response_data = msg.data[1:] if len(list(response_data)) == 0: return + if frame_type in range(0x41, 0x46): + self.position_feedback.accept(frame_type, response_data, msg.timestamp) # 并联控制指令响应 if frame_type == 0x01: self.x01 = list(response_data) elif frame_type == 0x02: self.x02 = list(response_data) @@ -1040,6 +1047,9 @@ class LinkerHandG20Can: return None return self.joint_state_to_cmd_state(state=state) + def get_feedback_snapshot(self): + return self.position_feedback.snapshot() + def get_current_pub_status(self): """API接口:获取手指当前状态""" self.get_current_status() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py index de94aed..8a3054a 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py @@ -6,6 +6,7 @@ import threading import numpy as np from utils.open_can import OpenCan from utils.color_msg import ColorMsg +from utils.feedback import ReceivedPositionFrames from can.exceptions import CanError @@ -17,6 +18,7 @@ class LinkerHandL6Can: self.open_can = OpenCan(load_yaml=yaml) self.x01 = [0] * 6 # 关节位置 + self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)]) self.x02 = [-1] * 6 # 转矩限制 self.x05 = [0] * 6 # 速度 self.x07 = [-1] * 6 # 加速度 @@ -233,6 +235,7 @@ class LinkerHandL6Can: break if not is_position_echo: self.x01 = list(response) + self.position_feedback.accept(frame_type, response, msg.timestamp) elif frame_type == 0x02: # 0x02 self.x02 = list(response_data) elif frame_type == 0x05: # Set speed @@ -326,6 +329,9 @@ class LinkerHandL6Can: def get_current_pub_status(self): return self.x01 + def get_feedback_snapshot(self): + return self.position_feedback.snapshot() + def get_speed(self): #self.send_frame(0x05, [],sleep=0.003) #print("L6暂不支持读取实时速度") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py index 5c95ab8..4a1a256 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py @@ -4,6 +4,7 @@ import threading import numpy as np from utils.open_can import OpenCan from utils.color_msg import ColorMsg +from utils.feedback import ReceivedPositionFrames from can.exceptions import CanError @@ -15,6 +16,7 @@ class LinkerHandO6Can: self.open_can = OpenCan(load_yaml=yaml) self.x01 = [0] * 6 # 关节位置 + self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)]) self.x02 = [-1] * 6 # 转矩限制 self.x05 = [0] * 6 # 速度 self.x07 = [-1] * 6 # 加速度 @@ -224,6 +226,7 @@ class LinkerHandO6Can: return if frame_type == 0x01: # 0x01 self.x01 = list(response_data) + self.position_feedback.accept(frame_type, response_data, msg.timestamp) elif frame_type == 0x02: # 0x02 self.x02 = list(response_data) elif frame_type == 0x05: # Set speed @@ -317,6 +320,9 @@ class LinkerHandO6Can: def get_current_pub_status(self): return self.x01 + def get_feedback_snapshot(self): + return self.position_feedback.snapshot() + def get_speed(self): self.send_frame(0x05, [],sleep=0.002) #print("L6暂不支持读取实时速度") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py index 82fc7e7..3074b82 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py @@ -207,6 +207,14 @@ class LinkerHandApi: getter = getattr(self.hand, "get_cached_current_status", None) return getter() if getter is not None else None + @property + def supports_feedback_timestamps(self): + return callable(getattr(self.hand, "get_feedback_snapshot", None)) + + def get_feedback_snapshot(self): + getter = getattr(self.hand, "get_feedback_snapshot", None) + return getter() if getter is not None else None + def get_state_for_pub(self): return self.hand.get_current_pub_status() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/feedback.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/feedback.py new file mode 100644 index 0000000..18ce8f0 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/feedback.py @@ -0,0 +1,65 @@ +"""Atomic CAN position snapshots with the original receive timestamps.""" + +from dataclasses import dataclass +import math +from threading import Lock + + +@dataclass(frozen=True) +class FeedbackSnapshot: + positions: tuple + channel_stamps_ns: tuple[int, ...] + + @property + def stamp_ns(self): + return max(self.channel_stamps_ns) + + def as_dict(self, names): + return {"schema": "can_feedback_v1", "name": list(names), + "position": list(self.positions), "channel_stamps_ns": list(self.channel_stamps_ns), + "stamp_ns": self.stamp_ns} + + +class ReceivedPositionFrames: + """Keep receipt identity even if stationary feedback has identical values. + + ``layout`` lists the frame and value index for each public SDK channel. + None represents a protocol-reserved constant-zero slot, whose freshness + is bounded by the oldest actual frame. No polling/publication clock is + allowed to update a snapshot's timestamps. + """ + + def __init__(self, sizes, layout): + self.sizes, self.layout = dict(sizes), tuple(layout) + self._frames = {} + self._lock = Lock() + + def accept(self, frame, values, timestamp): + if frame not in self.sizes or len(values) != self.sizes[frame]: + return False + if not isinstance(timestamp, (int, float)) or not math.isfinite(timestamp) or timestamp <= 0: + return False + stamp = int(round(timestamp * 1_000_000_000)) + with self._lock: + previous = self._frames.get(frame) + if previous is not None and stamp <= previous[0]: + return False + self._frames[frame] = (stamp, tuple(values)) + return True + + def snapshot(self): + with self._lock: + if self._frames.keys() != self.sizes.keys(): + return None + oldest = min(value[0] for value in self._frames.values()) + positions, stamps = [], [] + for item in self.layout: + if item is None: + positions.append(0) + stamps.append(oldest) + else: + frame, index = item + stamp, values = self._frames[frame] + positions.append(values[index]) + stamps.append(stamp) + return FeedbackSnapshot(tuple(positions), tuple(stamps)) diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py index b08da9d..502130d 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py @@ -168,6 +168,8 @@ class LinkerHand(Node): self.last_position_command_time = None self.last_hand_state = [-1] * 10 + self.last_hand_state_stamp_ns = None + self._last_timed_feedback_stamps = None self.last_hand_vel = [-1] * 10 self.force = [[-1] * 5] * 4 self.matrix_dic = { @@ -245,6 +247,7 @@ class LinkerHand(Node): COMMAND_QOS, ) self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10) + self.timed_state_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_state_timed'), 30) self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10) if self.is_touch == True: if self.modbus != "None": @@ -391,7 +394,7 @@ class LinkerHand(Node): self.last_hand_vel_cmd = None def _poll_state_if_due(self): - if self.hand_state_pub.get_subscription_count() < 1: + if self.hand_state_pub.get_subscription_count() < 1 and self.timed_state_pub.get_subscription_count() < 1: return now = time.monotonic() if state_reads_deferred( @@ -400,8 +403,7 @@ class LinkerHand(Node): self.command_quiet_period, self.defer_state_reads_while_commanding, ): - # pub_state continues to publish the last completed state as a - # heartbeat. A fresh blocking read is made after motion settles. + # A repeated heartbeat retains its original measurement time. return if not state_poll_due( self.last_state_poll_time, now, self.state_poll_period @@ -411,6 +413,7 @@ class LinkerHand(Node): # another read on the following timer callback. self.last_state_poll_time = now self.last_hand_state = self.api.get_state() + self.last_hand_state_stamp_ns = self.get_clock().now().nanoseconds time.sleep(0.003) if state_poll_due( self.last_velocity_poll_time, now, self.velocity_poll_period @@ -478,9 +481,7 @@ class LinkerHand(Node): def pub_state(self): while True: - if self.hand_state_pub.get_subscription_count() > 0: - msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) - self.hand_state_pub.publish(msg) + self._publish_feedback() if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0: msg = Float32MultiArray() msg.data = [float(val) for sublist in self.force for val in sublist] @@ -498,6 +499,26 @@ class LinkerHand(Node): self.hand_info_pub.publish(msg) time.sleep(self.hz) + def _publish_feedback(self): + """Publish one atomic measured state; never give cached values a new time.""" + if self.api.supports_feedback_timestamps: + snapshot = self.api.get_feedback_snapshot() + if snapshot is None: + return + positions, stamp = snapshot.positions, snapshot.stamp_ns + if (snapshot.channel_stamps_ns != self._last_timed_feedback_stamps + and self.timed_state_pub.get_subscription_count() > 0): + message = String() + message.data = json.dumps(snapshot.as_dict(self.api.get_finger_order())) + self.timed_state_pub.publish(message) + self._last_timed_feedback_stamps = snapshot.channel_stamps_ns + else: + positions, stamp = self.last_hand_state, self.last_hand_state_stamp_ns + if stamp is None: + return + if self.hand_state_pub.get_subscription_count() > 0: + self.hand_state_pub.publish(self.joint_state_msg(positions, self.last_hand_vel, stamp_ns=stamp)) + def pub_matrix_mass(self, dic): """发布矩阵数据合值 单位g 克 JSON格式""" msg = String() @@ -556,10 +577,12 @@ class LinkerHand(Node): msg.data = json.dumps(self.matrix_dic) self.matrix_touch_pub.publish(msg) - def joint_state_msg(self, pose,vel=[]): + def joint_state_msg(self, pose, vel=(), *, stamp_ns=None): joint_state = JointState() joint_state.header = Header() - joint_state.header.stamp = self.get_clock().now().to_msg() + if stamp_ns is None: + stamp_ns = self.get_clock().now().nanoseconds + joint_state.header.stamp.sec, joint_state.header.stamp.nanosec = divmod(int(stamp_ns), 1_000_000_000) joint_state.name = self.api.get_finger_order() joint_state.position = [float(x) for x in pose] if len(vel) > 1: diff --git a/src/linker_hand_ros2_sdk/test/test_feedback_timestamps.py b/src/linker_hand_ros2_sdk/test/test_feedback_timestamps.py new file mode 100644 index 0000000..c9315c5 --- /dev/null +++ b/src/linker_hand_ros2_sdk/test/test_feedback_timestamps.py @@ -0,0 +1,73 @@ +"""A CAN receipt, rather than a heartbeat, defines position sample identity.""" + +import json +from types import SimpleNamespace + +from linker_hand_ros2_sdk.LinkerHand.utils.feedback import ReceivedPositionFrames +from linker_hand_ros2_sdk.linker_hand import LinkerHand + + +def test_cached_unchanged_and_out_of_order_frames_keep_measurement_identity(): + frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)]) + assert frames.snapshot() is None + assert not frames.accept(1, [4], 1.) + assert not frames.accept(1, [4, 5], 0.) + assert frames.accept(1, [4, 5], 1.) + first = frames.snapshot() + assert first == frames.snapshot() + assert not frames.accept(1, [8, 9], .9) + assert not frames.accept(1, [8, 9], 1.) + assert frames.snapshot() == first + assert frames.accept(1, [4, 5], 2.) # A new stationary observation is real. + assert frames.snapshot().positions == first.positions + assert frames.snapshot().stamp_ns == 2_000_000_000 + assert first.stamp_ns == 1_000_000_000 + + +def test_split_finger_snapshot_does_not_retimestamp_held_channels(): + frames = ReceivedPositionFrames({1: 2, 2: 2}, [(2, 1), (1, 0), None]) + frames.accept(1, [10, 11], 1.) + assert frames.snapshot() is None + frames.accept(2, [20, 21], 1.02) + first = frames.snapshot() + assert first.positions == (21, 10, 0) + assert first.channel_stamps_ns == (1_020_000_000, 1_000_000_000, 1_000_000_000) + frames.accept(2, [20, 22], 1.04) + assert frames.snapshot().channel_stamps_ns == (1_040_000_000, 1_000_000_000, 1_000_000_000) + + +def test_sdk_republication_cannot_refresh_cached_state_timestamps(): + frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)]) + joint_messages, timed_messages = [], [] + publisher = lambda messages: SimpleNamespace(get_subscription_count=lambda: 1, publish=messages.append) + host = SimpleNamespace(api=SimpleNamespace(supports_feedback_timestamps=True, + get_feedback_snapshot=frames.snapshot, get_finger_order=lambda: ['a', 'b']), + _last_timed_feedback_stamps=None, last_hand_vel=[], hand_state_pub=publisher(joint_messages), + timed_state_pub=publisher(timed_messages)) + host.joint_state_msg = lambda *args, **kwargs: LinkerHand.joint_state_msg(host, *args, **kwargs) + LinkerHand._publish_feedback(host) + assert not joint_messages and not timed_messages + frames.accept(1, [10, 20], 100.) + LinkerHand._publish_feedback(host) + LinkerHand._publish_feedback(host) + assert len(joint_messages) == 2 and len(timed_messages) == 1 + assert all(message.header.stamp.sec == 100 for message in joint_messages) + assert json.loads(timed_messages[0].data)['channel_stamps_ns'] == [100_000_000_000]*2 + frames.accept(1, [10, 20], 101.) + LinkerHand._publish_feedback(host) + assert len(timed_messages) == 2 and joint_messages[-1].header.stamp.sec == 101 + + +def test_g20_timed_layout_matches_the_public_sdk_order(): + from linker_hand_ros2_sdk.LinkerHand.core.can.linker_hand_g20_can import LinkerHandG20Can + hand = LinkerHandG20Can.__new__(LinkerHandG20Can) + layout = hand.joint_state_to_cmd_state([[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)]) + frames = ReceivedPositionFrames({frame: 6 for frame in range(0x41, 0x46)}, [item or None for item in layout]) + raw = [] + for finger, frame in enumerate(range(0x41, 0x46)): + raw.append([finger*10+i for i in range(6)]) + frames.accept(frame, raw[-1], 100.+finger*.01) + snapshot = frames.snapshot() + assert snapshot.positions == tuple(hand.joint_state_to_cmd_state(raw)) + assert snapshot.channel_stamps_ns[0] == 100_000_000_000 + assert snapshot.channel_stamps_ns[4] == 100_040_000_000 diff --git a/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py b/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py index 15c8cd8..626290c 100644 --- a/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py +++ b/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py @@ -46,12 +46,14 @@ def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None: sys.path.insert(0, str(linker_hand_root)) try: from core.can.linker_hand_l6_can import LinkerHandL6Can + from utils.feedback import ReceivedPositionFrames finally: sys.path.remove(str(linker_hand_root)) hand = LinkerHandL6Can.__new__(LinkerHandL6Can) hand.can_id = 0x27 hand.x01 = [10, 20, 30, 40, 50, 60] + hand.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)]) hand._position_echo_lock = threading.Lock() command = (255, 2, 253, 253, 253, 253) hand._pending_position_echoes = deque( @@ -60,13 +62,14 @@ def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None: hand._position_echo_timeout_seconds = 0.02 hand.process_response( - SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command))) + SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)), timestamp=100.) ) assert hand.x01 == [10, 20, 30, 40, 50, 60] assert not hand._pending_position_echoes measured = (250, 3, 252, 252, 252, 252) hand.process_response( - SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured))) + SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)), timestamp=101.) ) assert hand.x01 == list(measured) + assert hand.get_feedback_snapshot().channel_stamps_ns == (101_000_000_000,) * 6 diff --git a/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md b/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md new file mode 100644 index 0000000..ea78d21 --- /dev/null +++ b/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md @@ -0,0 +1,937 @@ +# 四轮采集与图像拟合的修改前评估 + +日期：2026-09-16。目标：稳定完成采集，从可信标定 JSON 重建修正 URDF。 + +## 当前结论（2026-09-16） + +当前 O6 按重新实测的 16.5 mm Tag 完成新采集，完整数据经正式离线流程验收通过， +已生成合格 JSON、修正 URDF 和发布清单。本次在线收尾发生系统内存耗尽，不能把离线 +成功表述为在线全过程无中断。已修正收尾读取原始日志时的整文件副本开销，独立验证见后文。 +没有增加 O6 专用容错分支或降低物理精度门限，也不据此宣称其他型号实机均已通过。 + +| 环节 | 当前证据与结论 | +| --- | --- | +| 四轮连续采集与必要稳态点 | 新会话 `20260916_105528` 完成 36/36，1 个小指方向自动重扫后通过；采集约 6 分 47 秒。随后收尾时发生 OOM，反馈超时暂停；完整数据已用于正式离线验收。此前两次 16 mm 会话也完成采集，但空间验收失败。 | +| 反馈时间 | 使用真实 CAN 接收时间；相同缓存不得伪装成新测量。逐通道时间保留至图像插值。 | +| 相机时间 | 三台实际设备的曝光事件与帧计数器吻合；设备锁存换算曝光中点，替换约晚 16.5 ms 的取帧时间。 | +| 调度 | ROS 控制/反馈顺序处理，图像独立有界队列；实机对照消除了共享锁造成的反馈排队。 | +| 映射输入 | 当前报告的分段线性反馈曲线保留曝光时刻的小数输入；实际字节命令与旧整数表保持量化。 | +| 图像证据 | 同一运动保存全部机位的原始角点，不依赖任务主机位或 PnP；最新会话保存 16,822 个图像记录。没有增加运动或停点。 | +| 图像验收 | 独立三维测量对原始角点仍须 ≤1.5 px；最终文件按原角度/位置门限检查，额外视角只提供物理容差内的图像相容性证据。已知真值反例和反向拒绝测试通过。 | +| 平行转轴零位 | 已修正参考点沿轴移动导致零位变化的公共公式；四型号源 URDF 的已知真值及任意轴上参考点检查通过。数学规则不再由型号开关选择。 | +| JSON → URDF | `20260916_105528/offline_165mm` 已生成通过验收的文件与 manifest；另外调用重建 CLI，仅从保存 JSON 和源 URDF 得到的文件与发布 URDF 逐字节相同。 | +| 整链空间精度 | 新 16.5 mm 会话已通过原门限。主观测运动/稳态独立验收中，各角色最差角度 P95 为 1.873°，位置 P95 为 2.122 mm；最终文件空间与图像检查通过。精度适用范围是观测关节、扫描方向与固定的其他关节姿态；未独立观测的关节仍按配置迁移。 | +| 尺度输入核验 | 用户实测更正为 **16.5×16.5 mm** Tag、**27×27 mm** 棋盘，与独立图像检查基本吻合。当前 O6 的 Profile、检测器和标定节点尺寸已同步并记录新指纹；未修改相机参数，未将旧 16 mm 数据冒充新尺寸采集。 | + +先前的分机位空间不一致已在修正实测尺寸后的新数据上通过验收，未靠增加拟合参数 +或放宽门限达成。当前稳定性问题集中在已确认的收尾内存峰值；保留完整采集数据直接 +验证收尾修复，不为重复计算重新运动机械手。其余型号仍需对应硬件实测，O6 当前 Tag +布局中的其他三指曲线迁移不等于各关节独立标定。 + +上述两次未验收候选的三个主动零位差均小于 0.03°，说明当前分段求解可重复， +不能据此证明其绝对空间精度。正式发布仍必须通过整链与最终文件验收。 + +以下按发生顺序保留评估依据；早期 CAN 未连接等现场状态已由上述新实采结果更新。 + +## 已核实的问题 + +1. 当前生产采集要求准备运动先确定唯一 PnP/铰链姿态；不唯一时整场暂停，后续三轮训练数据尚未获得。 +2. 准备阶段的自由铰链几何与最终保留骨长、轴向的 URDF 使用不同参数空间。 +3. 后续姿态已受准备模型约束，不能把其重复性直接解释为原始图像上的整链正确性。 +4. JSON 保存后读取并重建 URDF 已有独立字节一致性验证，应保留这条文件链。 + +## 修改前的离线实验 + +没有改动生产算法、现场硬件、原始采样或原始 URDF。以 `20260915_150015` 保存的原始 +角点进行诊断：前三轮抽取 480 个 Tag 观测，第四轮 160 个观测仅用于冻结后的检查。 +直接按源 URDF 的运动链拟合一个共同基座、三个允许修改的零位和每个 Tag 的固定安装； +另一个实验允许带固定基准的双向曲线修正。两个实验均使用记录时的相机参数。 + +- 固定曲线：39 个参数，32 次迭代收敛，约 0.60 秒。 +- 联合曲线：119 个参数，23 次迭代收敛，约 1.90 秒，曲线最大修正约 1.38°。 +- 带 0.05 px 噪声的已知真值实验，三个初值均收敛，三个零位最大误差约 0.023°。 +- 实采第四轮仍有不合格残差；小指末节最大约 4.15 px。上述实验不是实机通过证据。 +- 使用后来核验的相机参数进行的私有反事实诊断也没有消除全部残差，不能将旧数据改挂新参数发布。 + +脚本、参数、雅可比和报告位于 +`calibration_output/O6_RIGHT_001/20260915_150015/review/architecture_assessment/`。 +原型只证明统一参数空间可以计算、在所测合成条件下可恢复；没有证明全型号实机精度。 + +## 采用的边界 + +- 采集保存原始角点、相机身份、SDK 指令/反馈、方向、轮次与基准图像；固定基准漂移、 + SDK 故障、时间同步和必要采样覆盖检查仍然有效。 +- 稳定基准图像先冻结，三维解释后求解；未求解的图像不得标成已测准的角度或零位。 +- 训练只使用前三轮；第四轮与独立稳态验证不参与几何、安装、曲线或候选的选择。 +- 图像拟合使用与输出 URDF 一致的拓扑和可修改参数；不自动改变骨长、转轴、相机参数， + 不新增 CAD 零位假设。显式复制关节继续保留来源声明。 +- 像素残差小不是角度/位置精度合格的充分条件。原物理门限、参数可确定性和最终文件读取验证需保留。 +- 模型尚未通过回归与实测验收前，不凭局部收敛结果替换正式发布逻辑，不降低门限以生成 PASS。 +- 采集完成后的拟合失败应保留完整数据并支持离线重算，区别于运动中暂停。 + +## 第二轮评估：不采用未经验证的整体替换 + +对同一会话进一步测试直接从原始角点初始化、共同拟合 33 节点双向单调曲线的版本。 +共 359 个参数、6160 个训练残差，150 次迭代、43.39 秒后仍未收敛；拇指末节 +第四轮最大误差约 45.18 px，小指末节约 7.77 px。这个结果说明，前一个实验依赖 +既有拟合初值的局部收敛，不足以证明新流程具备独立初始化和实机鲁棒性。 + +因此该原型**未接入生产**，相关采集/基准/拟合草稿已移出程序源码，连同源代码快照保存到 +上述评估目录的 `unadopted_prototype/` 和 `prototype_source_snapshot.tar.gz`。 +`monotonic_urdf_image_replay.json` 保留失败数据。下一步不能通过增加迭代次数、减小残差权重 +或放宽门限将其直接上线。 + +经过四型号 FK 对照测试的投影基础 `core/fitting/urdf_image.py` 和对应测试也归档到 +`unadopted_prototype/`，便于复查。生产源码不保留未接入的备用算法。 + +## 本次实际接入的流程修改 + +1. 四轮及稳态采集完成、数据同步落盘后，显式关闭图像采集，撤销尚未返回的图像回调。 + 拟合使用已经保存的图像与相机参数；此后相机消息/Tag 位置变化不再改变这份输入或触发 + 采集中暂停。运动采集期间的固定基准漂移检查保持生效。 +2. 反馈接收、SDK 故障保护和用户中止在后台拟合期间持续生效。 +3. 数值拟合、文件验收和后台阶段协议错误统一结束为 `FAILED`，保存失败阶段、原始数据路径、 + 完成/计划采集单元数和异常堆栈；不再写成采集中 `PAUSED`。 +4. 状态输出始终提供原始数据路径，拟合失败提供诊断路径，提示先离线复算。 + 失败结果不会填写已通过的 JSON/URDF 路径，也不会被迟到观测或清理用中止覆盖。 +5. 修复反馈曲线整链回放中丢失非活动关节到达方向的问题。比如下游关节正在弯曲， + 上游关节保持不动时，上游仍应使用其到达方向对应的回差曲线，不能自动改用双向均值。 + 反馈回放和稳态指令回放现在使用同一个方向解析函数；记录了不完整或非法方向时明确拒绝。 + 独立两轴合成数据证明：旧处理会使固定 Tag 看起来有超过 4° 的安装误差，修复后 + 训练和第四轮最终 URDF 回放恢复真值。当前旧 O6 数据的对应影响小于 0.03°，不是 + 该次实采大误差的主要原因。数值见 `held_joint_direction_effect.json`。 +6. 用户确认 O6 顶部 ID7 与正面 ID2 固定在同一末节后，明确设置其安装连杆为 + `rh_thumb_distal`，不再从侧摆任务名称推断为侧摆根部连杆。Profile 哈希同步更新； + 旧会话的配置身份保持原样。独立合成场景中的 O6 安装真值同步修正，其他型号不变。 + 单独侧摆且弯曲角固定时，错误连杆可以被固定安装矩阵吸收，因此不能声称该错误 + 一定解释先前全部残差；必须在正确绑定及完整关节状态下继续验证。 + +这组流程修改解决采集完成后继续受实时视觉影响和失败状态误导的问题；准备段的几何 +不一致另按下面的评估处理。保持原文件生成与发布验收链路。 + +## 第三轮评估：使用输出 URDF 已保留的几何约束 + +源 URDF 中相邻平行转轴的平行关系及垂直轴距，在任意关节零位修正下均不变。 +旧准备模型却允许这些量自由变化，求出的几何无法由最终保留骨长和轴向的 URDF 表达。 +因此只对真实 Tag 安装连杆、观测拓扑和源 URDF 均满足条件的相邻平行轴，消去两个 +自由轴倾角和一个自由轴距参数。相机、Tag 安装与逐帧角度仍由图像估计；不新增 +CAD 零位约定、不使用 SDK 角度作为拟合先验、不引入额外惩罚权重。 + +适用性由共同编译器判断，没有 O6 专用分支；不满足条件时保留原图像模型。 +冻结模型记录新的策略版本、约束及源 URDF 哈希，最终处理前从源文件重新推导验证。 +旧模型的序列化字节结构保持不变，避免破坏旧证据哈希。内存记录与磁盘记录均保留会话头。 +断点策略升级为 `confirmed_branch_v6_source_geometry`；旧自由几何会话仍可离线读取， +不能作为新约束流程的断点混入新采集。 + +对 `20260915_160040` 保存的小指准备段 104 帧原始角点： + +- 原自由模型返回 `image_motion_families_not_distinguishable`。 +- 使用源平行关系与 36.9995936 mm 轴距后，四个候选均在原 150 次迭代上限内收敛。 +- 最优两个候选收敛到相同完整 Tag 姿态，其他候选仍用原统计检验排除。 +- 最优候选留出帧最大误差约 0.696 px，低于原 1.5 px 门限；正式求解器约 4.69 秒返回成功。 +- 原像素、角度/轴心不确定性、候选等价性及显著性门限均未放宽。 + +独立合成图像验证同向及反向平行轴的真值恢复；四型号源几何验证约束在独立零位变化下 +仍成立。这证明该准备段可以使用与输出一致的模型解算，**尚不等于新四轮整手实机通过**。 +结果保存在 `20260915_160040/review/cad_geometry_resolution.json` 及相邻诊断文件。 + +随后经实际 `BranchInitialization` 入口回放该会话三个准备段的原始图像，拇指侧摆 +98 帧、拇指弯曲 88 帧、小指 104 帧均通过，耗时约 0.45 / 3.78 / 4.94 秒。 +该回放使用用户已确认的当前 Tag 绑定，仅保存诊断；没有改写旧会话或发布新标定。 +见 `all_preparations_source_geometry.json` 和同目录可复现脚本。 + +## 验证顺序 + +1. 受约束图像模型的已知真值、多初值、旧证据兼容和错误几何反例。 +2. 四型号共同采集逻辑与数据身份；反馈/固定基准保护及中止持续生效。 +3. 前三轮训练身份与第四轮隔离；完整性、重复图像、篡改与旧会话身份检查。 +4. JSON 落盘后独立重建 URDF、标准 ROS 加载及最终文件验收。 +5. O6 当前相机配置下重新实采；只有通过实际验收才更新发布结果。 + +## 本轮验证结果及实机阻塞 + +- 最终几何、真实图像入口、采集边界、来源身份、JSON 独立重建回归：87 项通过（39.81 秒）。 +- 后台进程、诊断采集、四型号产物与 launch 相关集成检查：80 项通过（157.08 秒）。 +- 新旧断点策略、恢复与原始图像凭据检查：92 项通过（4.85 秒）。 + 各组有交叉，不将数量相加作为独立测试总数。 +- 已知真值覆盖两/三关节链和正/反向平行轴；故意把 37 mm 轴距改为 80 mm 时， + 求解器因原重投影门限不合格而拒绝，不能仅凭源几何声明强行通过。 +- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。 +- 用户确认现场为 O6 后，清理了仍订阅同一控制话题的 O30 残留 SDK/GUI。 + 实际执行新的 `calibrate_hand --no-resume` 命令时，启动检查报告 `CAN interface does not exist: can0`。 + 系统仅有 lo/eno1/wlo1；采集未开始、未发送电机指令、没有发布本轮新标定。 + 需恢复 O6 原通信适配器后继续四轮实测；当前不能宣称整手实机精度或全部型号实机通过。 + 启动报告保存在上述 review 目录的 `o6_cad_geometry_startup.json` 和日志中。 + +## 第四轮评估：有限恢复、精简停点与最终原始角点验收 + +本次优化以保留四轮连续扫描、独立验证集和原精度门限为前提。没有采用直接用运动中 +角度代替静态指令映射的办法，因为反馈滞后和方向回差会使两种角度不同。 + +### 准备段恢复 + +`runtime/zero_recovery.py` 为每组尚未冻结零位的关节提供一次局部恢复额度。 +图像数量不足或候选暂时不确定时，复用该组原归零路径补采；几何已确认而零位图像 +不足时，仅保持当前姿态重新打开图像窗口。恢复撤销旧观测回调，保留真实日志； +不替换已冻结几何/零位、不改变保持条件，不将设备故障当成视觉缺样处理。 +两种恢复共享一次额度，持续失败仍暂停，不无限重做整场四轮运动。 + +### 训练停点精简 + +`core/fitting/command_sampling.py` 只读取前三轮连续数据，在原静态训练网格上求最少 +保留节点，使每关节、每方向在原网格节点的插值变化不超过 0.25°。端点、baseline、 +中间支撑点和显式非线性补充节点必须保留；训练不足或单调修正过大时保留完整网格。 +该准则决定采样位置，不证明实际静态误差已经达标。每个保留点仍需采集新稳态图像； +原独立稳态验证点全部保留，只有这些验证与最终产物验收通过才可发布。 + +计划记录训练来源及自身哈希，采集、恢复、离线回放和拟合共同消费同一声明，禁止 +使用第四轮或稳态验证点反过来选择训练节点。断点策略升级为 +`confirmed_branch_v7_final_image_sampling`，旧数据不会混入新流程冒充新证据。 + +另一项优化只省去重复准备等待：上一段已经完成到位检查、下一目标的完整指令向量 +相同、一秒内反馈未变化时复用该结果；真实位移、反馈变化、记录过期或归零准备段 +仍执行原运动检查。原运动速度、避让和采样稳定性门限保持不变。 + +对 O6 `20260915_150015` 的已保存数据离线比较，稳态训练点由每方向 9/9/10 个变为 +7/6/10 个；独立验证仍为 10/10/11 个，双向总停点由 118 次降至 108 次。 +小指因训练曲线噪声保留原网格，不以强行减点换取精度风险。 +原方案和精简方案均通过原独立稳态角度验证，全部关节最大误差仍小于 1.69°。 +这是旧数据的训练点选择对照，不能当作新四轮实机或最终空间精度通过。 +原始文件哈希保持不变，脚本及结果为该会话的 +`review/assess_static_sampling.py`、`review/static_sampling_assessment.json`。 + +### 最终文件到原始图像 + +`runtime/artifacts/image_evidence.py` 从原始记录绑定图像身份、四角、Tag 尺寸以及 +受哈希保护的相机外参和 CameraInfo。`core/urdf/image_acceptance.py` 读取最终文件， +使用冻结的安装和基座，把 JSON＋URDF 的 FK 直接投影到第四轮及独立稳态验证四角。 +这里不调用 PnP、优化器或重新配准；同一最终产物还须通过原来的 3D 和角度门限。 +每帧四角重投影 RMS 沿用 1.5 px 门限，超限保存 `final_image_diagnostics.json`。 +原始证据缺失、相机身份变化、训练/验证重叠及重复图像都会被拒绝。 + +新软件的自动测试与离线数据检查不代表四型号已经逐一实测通过。 +O6 仍需恢复 `can0` 后运行新的完整四轮，并通过上述最终文件验收。 + +本次新增链路的验证结果： + +- 四型号精简采样策略及实际 JSON→URDF→角点验收共 11 项通过（96.41 秒）， + 包含静态验证异常仍被拒绝、第四轮不影响训练点选择等反例。 +- 局部恢复、真实定时器流程和重复等待边界共 24 项通过（21.62 秒）。 + 持续缺少第十张零位图像在一次补采后仍停止，不能少采冒充合格。 +- 最终角点、真实相机来源、混合实测/复制产物及后台进程共 23 项通过（25.13 秒）。 +- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过（1.81 秒）。 +- 构建后再次执行 O6 正式 `calibrate_hand --no-resume`，仍在配置检查阶段报 + `CAN interface does not exist: can0`，没有进入运动采集；日志保存为 + `20260915_160040/review/o6_optimized_flow_startup.log`。 + +这些组存在覆盖交叉，不相加作为独立总数；四型号整链使用独立构造的合成观测， +真实来源绑定另用 O6 已保存角点检查，两者均不能冒充新实机结果。 + +全包回归中同步修正两处旧测试约定：零位不足的用例须观察到唯一一次恢复结束， +而非要求第一次超时立即暂停；历史侧面准备图像须调用带源 URDF 约束的当前生产入口， +不能继续要求旧自由几何模型必然可解。针对这两处的 7 项复测通过（11.49 秒）。 +同一份侧面历史数据 121 帧使用当前入口通过，训练/验证仍为互不重叠的 61/60 帧， +候选统计门限与绝对像素门限保持不变；没有为了通过测试修改原始数据或生产门限。 + +全包首次运行共 1497 项：1494 项通过、上述 2 项旧测试断言失败，另有 1 项因未提供 +O12 本地实采与独立参考文件而跳过（1175.15 秒）。两处失败修正后的 7 项复测通过， +没有剩余已知失败。完整日志、专项回归和构建日志保存在 +`20260915_160040/review/common_flow_validation/`；不将跳过的 O12 实采检查视为通过。 + +## 第五轮评估：训练选择与准备验证的边界 + +2026-09-15 19:32 使用新相机外参进行正式 O6 采集，拇指两任务完整采完， +小指准备阶段两次均报 `image_motion_families_not_distinguishable`。原始数据与修改前 +评估保存在 `20260915_193254/review/solver_assessment/`；先完成诊断实验，再修改生产代码。 + +整体链路保留：准备几何 → 冻结零位 → 四轮连续扫描 → 必要稳态映射 → JSON → 回读生成 +URDF → 独立图像验收。本次问题位于准备模型的选择和验证，不涉及末端非线性存储格式。 +独立稠密 Jacobian 求解证实增加迭代无效；四个初值有三个收敛到同一解，剩余解与其 +相差约 19°、9 mm，不能把真实空间差异当作浮点误差。 + +审查发现原实现按验证误差选择优胜模型，再用同一验证集证明它较好；拟合使用平方误差， +统计比较却使用 RMS 差；逐个立即返回还保留了最保守的固定多重比较惩罚。 +现统一为训练选择、验证否决，候选所用的训练资格不受验证结果或验证异常影响； +验证失败不得改选。公共 `image_model_selection.py` 按同一平方损失比较，完成全部候选 +比较后使用 Holm 校正，原始与校正后的概率均写入诊断。 + +1.5 px、0.03 px、1°/1 mm 等价界、原角度和空间验收门限均保留，名义族错误水平仍为 0.01。 +同帧等价检查按角色名对齐，并拒绝缺失或重复角色；不再依靠 `zip` 恰好有相同顺序。 +姿态策略升级为 `confirmed_branch_v8_training_model_selection`，旧会话仅供诊断，不能 +自动续接为新策略采集。新增策略适用于公共流程，不增加 O6 专用回退或停点。 + +修改前的候选策略诊断中，两次失败数据均可区分（概率约 0.00373、0.00576），拇指保持通过， +真正歧义的合成数据保持拒绝；这些仅是修复依据，不代表整手新实机验收完成。 +统计假设和条件性边界参考 [SciPy Wilcoxon 文档](https://docs.scipy.org/doc/scipy/reference/generated/scipy.stats.wilcoxon.html)， +多重比较方法参考 [Holm 方法文档](https://www.statsmodels.org/stable/generated/statsmodels.stats.multitest.multipletests.html)。 + +新策略实机 `20260915_195619` 完整采集 36/36 个方向单元，0 次暂停、0 次局部补采； +小指准备一次通过。146 项准备求解与产物链专项检查通过。最终空间验收仍失败， +候选 JSON/URDF 未发布；这与采集是否完整是两个不同结论。 + +## 第六轮评估：先修复测量时间，再判断空间求解 + +该会话的原始角点、生产重放和参数对照保存在 `20260915_195619/review/spatial_assessment/`。 +轴线投影开关没有改善整体误差；共同 URDF 的 39 参数图像求解虽然收敛，第四轮仍有 +4.124 px 误差，因此没有采用这一替换。相同反馈和相同方向的第四轮图像还能构成 +与任何确定性查找表不相容的反例：两帧像素距离 3.753 px，最大误差必有一帧不低于 +1.877 px，超过现有 1.5 px 门限。该反例不依赖拟合算法，也不证明硬件损坏。 + +SDK 代码确认了一个时间来源错误：发布线程给低频轮询缓存反复填上当前时间。 +相机与发布消息的时间差很小，并不能证明相机与真实反馈同步。修复位于共同数据边界： + +- CAN 接收端原子保存位置和接收时间，O6/L6 单帧、G20 按各指分帧保留。 +- SDK 的标准状态保留真实接收时间；标定专用接口携带每通道时间，仅发布新增测量快照。 +- 标定按每通道实际时间插值。重复、乱序、过期或同一接收身份对应不同值的记录不能刷新 + 稳态样本或反馈新鲜度；原始反馈快照落盘，便于复核。 +- 共用采集策略升级为 `unified_engine_v6_measured_feedback_time`，四型号 Profile 同步声明。 + 源 URDF、相机和精度门限保持原样；旧采集不能改挂新时间策略发布。 + +时间契约、缓存、分帧、回归与稳态检查 150 项通过；四型号配置、ROS 接入、断点与产物链 +集成检查 69 项通过；SDK 与标定两包构建通过。不运动的实机接口核验中，486 条标定反馈 +全部匹配独立旁路收到的原始 CAN 帧，0 条无来源、0 条重复身份。真实测量频率约 25 Hz， +同一时段标准接口发布 1147 条消息，说明之前约 58 Hz 的发布频率不能作为测量频率。 +该核验启动 SDK，SDK 仍执行其初始化速度/力矩设置；没有发送位置运动命令。 +完整实机产物仍须用新的测量时间重新采集后验收。 + + +## 第七轮评估：控制消息顺序处理，图像任务独立执行 + +真实时间采集 `20260915_202203` 暴露了图像缺样：一段约 0.30 秒的有效图像空白 +内仍有 15 条真实 CAN 测量，不应扩大同步窗口来掩盖。最初加入每视角有界图像工作线程， +但 ROS 端仍保留多线程执行器；`20260915_203104` 恢复导入期间仍因反馈超过一秒暂停。 +这项中间实现没有完成目标，不能算作修复成功。 + +进一步评估保存在 `20260915_203104/review/dispatch_profiling/`：真实 64 条记录编码 +约 6 ms，不支持直接减小导入条数。实机锁计时和线程栈显示，多个 ROS 回调争用同一把 +会话锁，图像队列还在持有自己的锁时反向查询会话。仅在诊断入口替换为单线程 ROS +执行器后，反馈进入回调的延迟由初段中位约 113 ms、P95 247 ms，降到约 9 ms、17 ms； +同一精度、采样和时间策略下已成功复用 8 个合格单元。 + +据此整理公共 ROS 接入，而不更换拟合器或增加型号特例： + +- 控制、反馈、相机消息解析和状态更新由一个 ROS 执行器顺序处理，删除冗余回调组。 +- 每个视角只保留一项执行中的图像任务和一项待处理任务，过载时保留最新待处理图像。 +- 只有 coordinator 接受的新测量才能推进图像队列的时间水位；多帧协议使用最旧通道时间。 + 队列持锁时不调用 coordinator，开始新会话时清空旧水位和未执行图像。 +- 原 50 ms 同步界限继续有效；图像估计仍在锁外，提交结果仍检查会话和运动版本。 +- 图像线程异常传回控制入口，不将线程停止当成正常无图；检查点落盘规则保持原样。 + +调度、ROS 接入、真实测量时间、coordinator 和断点恢复共 51 项针对性检查通过； +图像元数据并发、过期结果提交和隔离计算共 28 项检查通过；标定包构建通过。 +单线程对照运行及后续正式运行的终态以各会话记录为准；这里的延迟改善不能代替 +四轮完整性、独立角点和 JSON＋URDF 的最终空间验收。 + + +## 第八轮评估：相机曝光时间与取帧时间分离 + +单线程对照会话 `20260915_203750` 完成 36/36 单元（复用 8 个已验证单元）、0 次暂停， +生成候选 JSON 和由该 JSON 重建的 URDF，但顶部 ID7 的训练安装位置 P95 为 18.285 mm， +超过 3 mm，产物未发布。其余四个实测 Tag 角色通过该项空间检查。共同 URDF 的 39 参数 +原始角点拟合虽然收敛，第四轮仍达 4.779 px，因而没有替换生产拟合器。 + +随后检查图像时间来源，发现相机驱动在 `MV_CC_GetImageBuffer` 返回后使用当前 ROS +时间，不能表示曝光时刻。实测三台相机的设备时钟、40 次独立时钟锁存，以及曝光开始/ +结束事件，确认： + +- 三台取帧时间分别比设备帧时间晚约 16.44 / 16.52 / 16.49 ms；不应靠拟合器吸收该延迟。 +- 帧计数时间与独立曝光开始事件相差约 150 ns；固定曝光时可使用曝光中点时间配对。 +- 当前固件的 `DeviceTimestampIncrement` 返回 100,000,000，实测约 10 ns/tick。 + 设备 XML 的单位说明与此不一致，因此程序启动必须用独立经过时间核验计数单位。 + +`camera_timing.py` 单独负责设备/主机时钟对应和曝光时间换算。每 0.5 秒重新锁存， +选择往返区间最短的有效测量；偶发缓慢事务丢弃后有界重试，不能刷新原两秒有效期。 +错误单位、时钟跳变、计数器重置、重复帧和过期对应均不能伪装成新图像。 +相机驱动在图像及 CameraInfo 中使用同一曝光中点时间，保留每次锁存和每帧换算记录 +`camera_timing_.jsonl`。固定外参和内参不因软件时间修正而改变。 + +共用采集策略升级为 `unified_engine_v7_camera_device_time`，四型号 Profile 及其受保护 +哈希同步更新；旧会话保留诊断用途，不能直接续接到新时间规则。改动没有增加停点、 +调整非线性查找表语义或放宽最终精度门限。 + +时钟、真实三相机记录、ROS 接入、配置、启动图和断点/队列共 57 项检查通过；构建通过。 +随后实际驱动对三台相机各连续发布 242 帧、执行 16 次校时，无时序或时钟错误。 +O6 运动期间另旁路监听 CAN：370 条位置命令的响应均不是原命令回显，仍可作为真实状态 +观测；没有把 L6 的回显过滤规则未经核实套到 O6。 + +完整新策略实采会话为 `20260915_205704`，不复用断点、不使用调度诊断包装。 +共完成 36/36 个单元，0 次人工暂停；拇指弯曲第四轮增加方向和小指第三轮增加方向 +各自动重扫一次。候选 JSON/URDF 已生成，Tag 安装刚性训练检查失败，未发布 manifest。 +正式失败时位置 P95：小指末节 3.108 mm、拇指末节 6.476 mm、拇指弯曲 3.875 mm、 +顶部拇指 10.086 mm。会话中 `review/spatial_assessment/live_run_summary.json` 保留统计。 + +## 第九轮评估：反馈插值契约与空间模型边界 + +反馈拟合使用曝光时刻的连续插值值，报告声明 `piecewise_linear`；原 `JointMapping` +和 `SerializedJointMapping` 却对所有 `_u8` 输入再次取整，使训练和读取语义不同。 +统一规则为：现代反馈曲线连续插值，实际 `command_u8` 量化，历史整数表保持原兼容行为。 +序列化读取在量化前检查输入支持域，不能将越界小数取整后当成合法测量。 +此修改不改变非线性曲线本身、不重拟合零位、不调整采集版本或原始记录。 + +先用相同 39 参数整链诊断做对照：保留原 URDF 骨长、轴向和已拟合曲线，前三轮拟合 +共同基座、允许的零位及固定安装；独立第四轮不参与优化。取消反馈取整后，小指末节 +最大像素误差由 3.390 降至 2.449 px，但仍超过原 1.5 px，因此该诊断模型未接入生产。 +另加有界双向曲线修正的 119 参数实验，第四轮小指末节最大仍为 3.136 px，且出现 +非单调局部曲线；增加参数并没有提供足以替换现有生产求解器的证据。 + +正式公共回放也验证：反馈取整修复不能消除空间不一致。位置 P95 为小指末节 3.080 mm、 +拇指末节 6.489 mm、拇指弯曲 3.877 mm、顶部拇指 10.088 mm；保持失败且未发布。 +原始会话与在线候选文件未覆盖；新回放位于 `20260915_205704/review/spatial_assessment/` +下的 `continuous_production_replay/`。同目录保留诊断脚本、参数、Jacobian 和测试日志。 + +补充整链可行性对照仍只使用前三轮、相同 39 个参数：将训练目标换成按原 2°/3 mm +尺度归一化的三维位姿残差，局部收敛后拇指弯曲零位触及原 20° 边界；第四轮顶部 +位置 P95 仍为 5.148 mm，原始图像最大误差增至 28.718 px。此结果没有证明全局无解， +但否定了将这一局部三维优化结果直接用于替换现有求解器的做法。 +原始保存位姿经受保护的相机变换投回同帧角点，各角色最大误差均小于 0.64 px； +没有发现这条原始公共坐标变换的方向或图像身份错配。检查见 +`continuous_spatial.json`、`common_pose_projection_check.json`。 + +反馈输入、双映射、被动非线性与读取兼容检查 57 项通过；补充真实时间插值、四型号 +合成产物链、JSON 重建、独立角点和标准 URDF 检查 49 项通过，两组有交叉。 +SDK 和标定包最终构建通过，`git diff --check` 通过。 +合成验证不等于其他型号实机通过；本轮 O6 的最终空间精度问题仍未解决。 + +## 第十轮评估：保留跨运动的原始图像证据 + +此前原始角点随任务主机位的姿态采集写入，缺少顶部 ID7 在弯曲中、正面 ID2 在侧摆中 +的完整原始图像。这两块 Tag 已确认属于同一末节；只保存各自任务下的图像不足以独立 +核查共享运动链。公共采集器现独立保存所有机位的有效原始角点，保留原检测质量、 +窗口与会话身份，不依赖 PnP 是否成功。主采样门限、运动计划和稳态点数量均不变。 +同机位图像时间去重只在成功提交时推进；迟到或窗口已撤销的图像不能推进水位。 + +采集策略升级为 `unified_engine_v8_all_view_images`，四型号 Profile 及产品哈希同步更新。 +旧会话不修改哈希、不补造新策略记录。新增采集与公共并发检查 43 项通过，补充采集、 +四型号配置与断点身份检查 58 项通过，两组有交叉；标定包构建通过。 + +新会话 `20260915_213337` 完成 36/36，0 次人工暂停；小指第二轮反向扫描自动重试一次。 +三机位共保存 16,822 个原始图像记录。正式算法仍在 Tag 安装空间一致性处失败， +未发布 manifest。主动零位与上一会话差均小于 0.03°，不能以重复性替代正确性。 + +保持当前源 URDF、相机参数和 Tag 尺寸，前三轮拟合共享基座、零位和每块 Tag 的固定安装， +第四轮检查的新全视角对照仍未通过。39 参数固定曲线模型的最大像素误差约 3.52 px； +359 参数双向单调曲线模型最大约 4.32 px。后者按 SDK 输入均匀选择训练图像， +不再按停留时间重复加权；非负曲线增量和参考输入零约束仍不足以消除残差。 + +静止姿态也出现约 1%～3% 的投影边长偏差，因此另做一次受限的统一骨长尺度诊断。 +其训练最优尺度约 1.0327，第四轮最大误差仍约 4.37 px；没有采用该尺度，没有修改 +源 URDF 或相机文件。原始公共位姿可重投回其源角点，暂未发现转换方向错误。 +上述原型和数值结果均仅保存在 `20260915_213337/review/whole_chain/`，未接入生产求解器。 + +仅用稳态共同视野核查 ID2/ID7 刚体关系时，训练 6 个姿态、留出 7 个姿态全为侧摆， +不能独立确定完整外参。连续扫描保存了两种动作的共同视野，可在原曝光时间上对齐后 +进行独立于 URDF 和曲线的外参一致性诊断；诊断结果不能自动覆盖已记录的相机参数。 + +## 第十一轮评估：尺度一致性与全机位最终图像验收 + +对 ID2/ID7 同末节约束做独立检查，不使用 URDF 尺寸、轴向或 SDK 查找表。 +连续图像按曝光时间对齐，线性/局部三次及留一插值的一致性误差不超过 0.1 px； +训练前三轮 96 个姿态，第四轮 32 个姿态仅检查冻结的参数。每帧刚体位姿作为局部未知量， +两个 Tag 之间始终只有一个固定安装变换。多初值只按训练平方误差选择。 + +| 诊断假设 | 第四轮 front/top 最大误差 | 结论 | +| --- | --- | --- | +| 记录的外参与两个 16 mm Tag | 1.28 / 1.69 px | 与原始图像仍有不一致。 | +| 自由估计 front/top 外参，Tag 均 16 mm | 0.44 / 0.48 px | 外参改变量约 2.43°/23.72 mm；条件三倍标准差约 1.96°/22.24 mm，不能作为精确外参替换。 | +| 记录外参，单独估计 ID7 尺度 | 0.84 / 0.49 px | 固定 ID2 为 16 mm 时 ID7 约 16.52 mm；不是两块 Tag 实测尺寸差的独立结论。 | +| 记录外参，同时估计两个 Tag 尺度 | 0.42 / 0.52 px | 有效边长 ID2 约 16.38 mm、ID7 约 16.52 mm；依赖现有相机模型和刚体/正方形假设，尚不能替代物理尺寸。 | + +自由外参不能解释此前保存的棋盘格图像：其最大误差约 12.93 px，原外参约 0.56 px。 +因此不能因为手指图像误差变小就认定相机移动或自动重写外参。只读采集当前静止画面后， +正面背景匹配显示约 3 px 变化，侧面约 1 px 内；顶部旧图被棋盘格遮满，没有足够共同背景， +不能判断顶部机位是否变化。该检查没有启动 SDK 或驱动手指。 + +另将整链中的所有相机基线或所有 Tag 尺寸分别只增加一个公共尺度参数：前三轮最优值 +约为 0.97226 和 1.03073。两者都明显降低拇指正面误差，但第四轮小指末节最大仍约 +4.29/4.27 px、顶部最大仍约 2.66/2.44 px；没有采用任何尺度修正。 +条件协方差未包含相机、打印尺寸和模型的系统误差，不能用其很小的数值宣称物理尺寸测准。 +已请求分别精测 ID2/ID7 黑框宽高及棋盘格跨五格长度，现有整数尺寸记录不足以区分尺度来源。 +结果在 `review/whole_chain/rigid_*_assessment.json`、`board_camera_comparison.json`、 +`static_background_comparison.json` 和 `all_view_*_scale.json`。 + +程序结构上，只有任务主机位参与最终图像验收会漏掉同一 Tag 在其他动作下的不一致。 +现将相机身份核验和原始图像索引集中在 `ImageEvidence`，反馈和稳态指令各自使用独立窗口。 +前三轮、未通过尝试和无同步反馈的原始图像不能成为最终验收测量；主观测必须在新原始记录 +中找到完全一致的角点、SDK 值和方向。其他动作中的原始角点也直接进入冻结 JSON/URDF 投影， +不伪造 PnP 位姿，不参与训练，不增加运动或停点。历史主图像读取接口保留原证据语义。 + +实际 O6 输入核对：反馈原有 1,818 个主观测全部一致，增加 3,270 个其他观测； +稳态指令原有 312 个主观测全部一致，增加 677 个观测。此结果仅证明证据绑定正确， +不代表最终空间精度通过。专项和现有产物流程 36 项通过，补充最终检查、四型号合成产物、 +JSON 重建及非有限输入反例 34 项通过；两组有交叉，不相加为独立测试总数。 +标定包构建通过；实际数据证据核对和测试日志保存在 +`review/whole_chain/all_view_final_source_verification.json` 与 `final_image_validation/`。 + +### 剩余运动残差的解释边界 + +固定上述未启用的候选相机尺度与整链几何，仅逐帧调整活动关节角度进行定位。 +小指两关节最大改变量约 0.63°/0.49°，侧摆约 0.38°；图像最大残差分别可降至 +0.43/0.94 px 和 0.47 px。最大原始残差集中在 SDK 约 244 的运动区间； +同一 Tag 静止、其他手指运动时的误差显著更小。 + +这项实验在每个被检查图像上估计局部角度，明确**不是独立第四轮验收**，也不证明 +其条件几何正确。它提示剩余像素误差中包含运动映射误差，不能据此继续修改骨长或零位。 +前三轮相同输入位置的角度改变量有约 0.1° 的轮间差，增加曲线节点也未必能消除。 +后续需分别评估反馈分辨率、允许的运动映射误差与图像测量误差；不能直接把 PnP 的 +像素测量门限等同于整条运动链的角度/位置精度要求。本轮没有修改任何验收门限。 +完整逐帧结果见 `review/whole_chain/local_angle_residuals.json`。 + +## 第十二轮评估：分离图像测量误差与运动精度 + +先用完全独立的已知真值验证规则：单关节运动存在恒定 0.5°、0.873 mm 偏差， +满足原角度和位置要求。仅将相机焦距从 1000 改为 3500 px，最终 FK 的最大投影误差 +就从 1.10 变成 3.84 px；原规则因此对相同物理精度作出不同结论。测量位姿对原始角点 +的误差均近于零。这证明 PnP 的 1.5 px 图像测量门限被误用于含运动映射误差的总投影量。 + +公共验收现在分为以下职责： + +1. 发布器原有的独立三维反馈、指令验收仍先执行，角度 MAE≤1°、P95≤2°、最大≤3°， + 位置 P95≤3 mm。非线性被动关节仍读取完整 JSON，不以 mimic 近似替代。 +2. `image_acceptance.py` 将独立位姿与原始角点、SDK 输入、方向和图像身份直接绑定。 + 测量位姿重投影 RMS≤1.5 px；最终 FK 的总投影误差保留为单独统计，不据此改变物理精度要求。 +3. 额外视角无独立三维测量时，`image_consistency.py` 查找一个能解释原始角点的局部刚性位姿， + 直接核验其像素残差及相对最终 FK 的物理位移。数值见证仅说明图像与容差相容， + **不是真实位姿测量，不证明最优，不替代独立三维精度**。求解器返回成功也不能单独使其通过。 + 不回写或重新估计共享的相机、安装、基座、零位和曲线；按运动任务分别统计，防止静止帧稀释误差。 +4. manifest 明确记录 `independent_measurement_and_physical_consistency_v2`， + 独立精度与额外机位相容性分开报告。缺失独立位姿不能由数值见证补齐。 + +解析投影 Jacobian 已用独立有限差分检查。已知真值、不同焦距、物理超差、角点/相机篡改、 +证据身份和静止图像稀释等 42 项专项检查通过；四型号合成产物、非线性、JSON 重建及原物理 +验收共 36 项通过；最后的规则版本/任务身份补充检查 15 项通过。测试集合有交叉，不相加。 +标定包构建通过，`git diff --check` 通过。没有增加实机轮次或停点。 + +最新真实 O6 的 1,818 个第四轮及 312 个稳态主观测均核验了真实保存位姿与原始角点， +最大误差分别为 0.613/0.616 px。此检查没有拟合任何参数，属于测量证据核验，不是产物 PASS。 +复算原训练安装检查仍得到拇指位置 P95 为 5.371/3.591/11.352 mm，仍失败，没有 manifest。 +尺度/相机输入尚待现场精测区分，未采用之前任何诊断尺度，也未直接替换整链求解器。 +已知真值脚本使用保存的旧规则源码复现反例，当前规则由生产测试验证；全部记录位于 +`20260915_213337/review/whole_chain/image_error_budget/`。 + +## 第十三轮评估：轴线参考点不能决定零位 + +一条转轴可用轴上任意一点表示；沿轴移动该点不会改变物理几何。当前 O6 的旧零位公式 +先把两轴参考点的差直接投到图像平面，没有消除沿轴任意坐标。相机斜看转轴时，这个 +任意坐标被误解释成弯曲零位。旧代码已为 O12 增加可选开关，但 O6/L6/G20 仍使用原公式。 + +在真实 O6 的相同轴线输入上，仅将被动轴参考点沿轴移动 ±20 mm：拇指弯曲零位从 +−9.8736° 变成 +1.0529° 或 −16.1650°，三个结果竟都通过原轴线角度检查。整个实验没有 +改变相机、Tag 尺寸、运动图像或真实轴线，足以证明程序中存在几何表示相关的错误。 + +公共 `ObservationGeometry.phase_error` 现始终先取两轴的垂直间距向量，再投影到相机平面。 +移除了 `project_axis_gauge_before_image` 型号开关及其编译参数；O12 删除已无必要的 YAML +声明并同步产品哈希，其数学行为保持原先已启用的正确规则。O6 的采集配置与受保护输入 +不变，可以用原四轮数据离线复算，无须增加运动。 + +独立测试使用四型号的原始 URDF、自行生成已知 0.07 rad 零位，改变相机视角、共同坐标系， +并任意移动预测和观测的轴上参考点；所有平行轴对均恢复相同真值。源码专项、四型号完整 +合成产物链、配置与 O12 安装/机位变化共 24 项检查通过；源代码的轴线/求解顺序 9 项通过。 +构建后另核对实际加载的安装包与源码字节一致，安装包的轴线、图像与物理门限检查共 +51 项通过，避免用旧安装文件检验新修改。各测试集合有交叉，不相加。 + +正式回放保留原始输入身份检查，输出到 `review/whole_chain/axis_gauge_production_replay/`。 +拇指弯曲零位约 −0.5384°、小指约 −1.8044°，不再依赖轴上参考点；侧摆仍约 14.5179°。 +JSON 已先保存并被读取生成候选 URDF；空间安装检查仍失败，位置 P95 如页首所列， +未生成 manifest。这项数学修正没有消除后续空间不一致，也不能替代尚待核实的尺度输入。 + +对最近两次已保存的轴线输入使用同一修正公式，侧摆、拇指弯曲、小指零位差分别为 +0.02955°、0.01044°、0.02942°。这是条件零位的重复性对照，不是绝对精度通过； +没有改写旧 v7 会话的采集策略或将其伪装成 v8 发布。结果见 `corrected_zero_repeatability.json`。 + +旧公式及配置已备份到 `axis_gauge_before/`，`assess_axis_line_gauge.py` 从备份源码复现 +历史反例；不将旧公式或型号条件保留在生产求解器中。数值对照见 `axis_line_gauge_assessment.json`， +源码、安装包测试及构建日志均在同一 `review/whole_chain/` 目录。 + +## 第十四轮评估：独立双目尺度检查 + +先使用已冻结的 ID2/ID7 刚体关系，在第四轮 32 个共同观测姿态上仅估计逐帧刚体位姿， +不使用 URDF 或 SDK 曲线。保持原相机与两个 16 mm Tag 时，front/top 图像最大误差仍为 +1.280/1.693 px；与原主机位三维测量的位置 P95 分别相差 26.84/19.09 mm。 +此前“调整外参”或“调整 Tag 尺寸”的条件模型均能把最大图像误差降至约 0.5 px， +但得到不同的三维位置，且部分与原主测量相差约 20～33 mm。因此不能将其中任意一个 +条件重建当成独立真值，亦不能用小图像残差证明最终空间精度。共享参数均未在第四轮更新。 +对照见 `stereo_primary_measurement_comparison.json`。 + +只读保存的静止画面还发现：侧面相机可以同时看见正面 ID1、ID2。用两机位对同一个 Tag +的四个对应角点直接三角测量，不预设 Tag 尺寸、刚体安装、URDF 或 LUT。两帧曝光相差 +6.55 ms，保存时没有 SDK 控制进程、机械手静止。尺度仅来自记录的 front/side 外参基线。 + +- ID1 两组对边平均长度为 **16.496 / 16.503 mm**；两机位重投影 RMS 为 **0.536 / 0.443 px**。 + 这项独立对照进一步提示当前尺度与“16 mm”的记录不一致。 +- ID2 两组对边约 **16.593 / 16.713 mm**，但重投影 RMS 为 **1.583 / 1.412 px**， + 最大单角误差约 2.28 px，不能将该尺寸估计作为已通过质量检查的测量。 + +此检查仍依赖棋盘格确定的相机基线尺度，不能独立区分打印 Tag 偏大与棋盘格尺度偏差。 +没有把 ID1 的尺寸复制给其他 Tag，没有更改原相机或尺寸配置，没有增加整手运动。 +原始 PNG、相机元数据、逐角三维坐标及误差均可复查，见 `current_camera_images/`、 +`same_tag_stereo_assessment.json` 与相应同名诊断脚本。 + +下一项必要现场证据仍是已请求的精确黑框尺寸与棋盘格跨五格长度。现有图像能发现 +尺度不一致，但不足以无假设地决定应修改哪一项物理输入。尺寸证据到位后，应先核实输入、 +评估与原始数据的兼容性，再复算整链；不能把某个局部拟合最优值直接写入生产配置。 + +## 第十五轮评估：固定已确认尺寸，排除图像坐标处理与局部拟合误导 + +2026-09-16 用户再次明确：全部 Tag 黑色码区为 16×16 mm，棋盘格为 27×27 mm。 +这解除上一轮的尺寸信息等待；本轮按用户确认值作为固定物理输入，不把双目条件估计 +当作新的 Tag 尺寸。以下诊断记录在 `20260915_213337/review/whole_chain/confirmed_dimensions_audit/`。 + +### 1. 原始图像与去畸变图像的数值核验 + +仅启动相机，机械手不运动；保存同一曝光时间戳的原始和去畸变 PNG。按 CameraInfo 的 +K、D、R、P 独立执行 OpenCV remap，front/side/top 三机位与 image_proc 输出的最大、平均 +灰度差均为 **0**。程序以 P 解释 image_rect、棋盘物点间距为 0.027 m；没有发现单位错误、 +缩放裁剪、重复去畸变或原始 K 与去畸变图像混用。该证据只核验软件坐标契约，不证明 +K/D 本身就是准确物理内参。结果见 `raw_rect_consistency.json`、`raw_rect_capture/`。 + +新静止图像中 ID1 的两组对边平均为 **16.5057/16.5078 mm**，两机位重投影 RMS 为 +**0.830/0.687 px**。ID2 的重投影 RMS 为 1.836/1.635 px，继续不作为合格尺度测量。 +因此上一轮发现的矛盾可再次观测，并非一次检测结果；见 `fresh_same_tag_stereo.json`。 + +### 2. 棋盘低像素误差不能单独决定新相机参数 + +使用已有原始角点，保持 27 mm 间距，对正面/侧面及正面/顶部训练对分别联合估计内参。 +两组的留出误差均下降，但同一 front 的主点横坐标分别约 866/1078 px，明显不同。 +又将单机位训练集分为互不重叠子集：side 的部分子集可给出明显不同焦距/主点，同时 +在同一留出图像上仍保持约 0.45～0.48 px 的 RMS。不能凭这个像素指标选择物理参数。 +结果见 `joint_intrinsics.json`、`intrinsic_stability.json`；未修改相机配置。 + +进一步以 OpenCV object-releasing 仅在训练图像中估计一个固定棋盘角点形状，保持 +第一行两端 7×27 mm 间距。三个机位独立估计的形状均存在亚毫米偏差；其中 front、side +估出的形状相近。冻结形状后，未参与拟合的单机位图像 RMS 分别从 +**0.276/0.465/0.317 px** 降为 **0.133/0.168/0.138 px**。 +这提示理想平面棋盘假设可能影响内参，但仍是条件模型证据，不等于已测准棋盘变形。 + +为避免“各相机自由拟合都能变好”的误判，另只采用 front 训练图像估出的同一形状， +重新估计 side/top 内参，再用 19:25 棋盘原角点训练新的条件外参（每四组留一组验证）。 +side 的留出双目 RMS 最大约 0.395/0.383 px；随后检查从未参加相机拟合的新静止 Tag， +ID1 两组对边仍为 **16.5038/16.5058 mm**。棋盘误差改善没有解决目标尺度矛盾，故明确 +拒绝启用这组候选参数。见 `board_shape.json`、`shape_camera_transfer.json`。 + +### 当前下一步 + +已请求保持手和相机不动，将棋盘置于手旁，使 front/side 同时看到完整棋盘及 ID1， +采集同一时段的静止画面。这可直接核查已确认的两种尺度，减少先标棋盘、后拍手之间 +场景变化的影响。另询问棋盘载体是否刚性平整，作为几何假设的现场证据。 +本轮不增加整手运动、不启用候选相机参数、不改变 Tag 尺寸或精度门限；原数据和受保护 +配置保留。整手发布条件仍未达成，无新的合格 manifest。 + +### 当日现场反馈与贴平后检查 + +用户补充“棋盘没有完全弯曲变形，tag贴纸有弯曲不平整”，随后确认“已贴平固定”。 +因此不能把之前的平面 Tag 位姿当作已证实准确的物理测量，也不能把棋盘条件拟合 +直接解释成全部误差的原因。正式复标需使用处理后的新图像，而非沿用旧 Tag 安装结果。 + +10:09 的贴平后静止对照，front/side 曝光差为 1.763 ms。ID2 的双目重投影 RMS 为 +1.349/1.201 px，较处理前改善；ID1 为 2.254/1.872 px，仍不满足图像质量门限。 +两者的条件尺度仍约 16.5 mm，不能据此宣布测量链修复完成。数据保存在 +`confirmed_dimensions_audit/flattened_tags_20260916_100921/`。 + +用户表示棋盘已摆好后进行了实际拍摄，并持续开启无机械手控制的双目预览；至 10:10 +两路画面实际均未出现棋盘。已提供正面/侧面画面帮助摆放，没有将空缺的棋盘观测标成 +成功采集。新的同帧棋盘/Tag 核验脚本已用已知真值验证：正确相机下恢复 16 mm 各边， +误差小于 1e−5 mm；该合成检查不是现场相机验证。下一步仍需让棋盘实际进入两个机位， +目前未重新启动整手运动或输出新的合格产物。诊断汇总与受保护输入哈希见 +`confirmed_dimensions_audit/audit_summary.json`。 + +## 第十六轮评估：分时核验棋盘与 Tag，取消不必要的同框限制 + +固定相机外参时，棋盘负责估计相机之间的刚体变换；Tag 的位置和姿态在检查时独立 +估计。因此棋盘与机械手不必同时出现在画面中，手在两次拍摄间也不必保持原位置。 +此前把同框设为必要条件，给现场带来了不必要的摆放限制，现已取消。只需相机及镜头 +设置在两段采集中保持固定。这一修改属于诊断采集流程，不改变整手正式运动流程。 + +10:21～10:22 保存四组正面/侧面完整棋盘原图： +`board_placement_preview_20260916_102131`、`102213`、`102215`、`102217`。 +原始双目曝光差 10.79～10.99 ms；每组以已确认的 27 mm 棋盘估计条件相机变换， +棋盘重投影 RMS 为 front 0.281～0.364 px、side 0.438～0.571 px。 +四次相对首帧的变化最大为 0.0272°、0.458 mm。姿态相近的四组只说明条件重复性， +不能充当完整多姿态外参标定，也没有写入活动相机配置。 + +临时预览的自动保存器还曾设置 20 ms 配对条件，而一次启动后的相机帧相位差约 +24.66 ms，造成没有进入检测。这是诊断脚本的问题，已采用原相机核验的 50 ms 配对 +范围，并保留真实曝光时间；正式图像与空间精度门限不变。另一个临时触发条件要求 +全部棋盘角点连续 2 秒变化均小于 0.4 px，未触发自动保存。上述四组是另行直接保存的 +原始画面，经离线图像核验后使用；不能把预览中的 `saved_count=0` 改记成自动触发成功。 +后续共面检查直接保存可见原图，在原图上评估几何质量，不先用这种触发条件丢弃证据。 + +用户移开棋盘、放回 O6 后，10:24 保存新的 Tag 图像，曝光差 11.75 ms，位于 +`joint_board_tag_20260916_102403/`。分别使用上述四组棋盘估计的相机变换，得到： + +- ID1 两组对边平均约 **16.487～16.500 mm**；自由角点双目重投影 RMS 为 + front 1.040～1.107 px、side 0.878～0.935 px。 +- 强制 ID1 为 16 mm 平面方形时，front RMS 仍为 **1.629～1.677 px**，超过 1.5 px。 +- ID2 两组对边约 **16.499～16.585 mm**；自由角点的双目 RMS 约 0.41～0.51 px。 + +新旧手位姿分别估计，不参与棋盘相机参数的拟合。使用已知真值的另一个位置/姿态 +Tag 验证过这一分时检查方法，两种来源均恢复 16 mm，误差小于 1e−5 mm。 +结果记录在各棋盘目录的 `board_tag_consistency_joint_board_tag_20260916_102403.json`。 +这仍不足以单独判定应改 Tag 尺寸、棋盘尺寸还是相机模型；没有因此运行新一轮整手运动。 + +### 共面长度比例的独立检查 + +用户提供同批备用 Tag，并把 ID1 固定在棋盘同一块板上。10:27 的三个原始正面图像 +位于 `coplanar_capture_20260916_102710/frame_00` 至 `frame_02`。图中左侧的备用 ID1 +与棋盘共面；右侧手掌 ID0 不共面，不将其按棋盘平面换算的数值解释为尺寸。 + +使用棋盘格交错的 20 个角点拟合平面单应变换，另 20 个角点检查投影误差，不使用 +相机外参、URDF、SDK 或关节曲线。原始像素直接计算的 ID1 两组对边约 +**16.474～16.478 / 16.595～16.602 mm**，留出棋盘角点 RMS 约 0.186～0.191 px。 +用当前内参去畸变后约 **16.680～16.685 / 16.593～16.600 mm**；旧内参、未采用的 +棋盘形状内参仅作预先规定的敏感性对照，结果也记录在同一报告中，不按 Tag 尺寸选模型。 + +该 ID1 接近原图左边缘，畸变模型之间仍有约 0.2 mm 的差异，所以未据此改配置。 +随后请用户将备用 ID1 移到棋盘上方中间，再核验边缘畸变的影响。 + +### 中央位置复核与实测尺寸的矛盾 + +用户将备用 ID1 贴到棋盘中间上方后，10:43 保存三个新的原始正面图像，位于 +`coplanar_capture_20260916_104349/frame_00` 至 `frame_02`。图中 ID1 位于同一底板上方， +纸张棋盘完整可见；手掌 ID0 仍不与棋盘共面，不解释其尺寸换算值。 + +沿用预先规定的棋盘交错训练/留出划分，三个图像的结果为： + +- 原始像素直接计算的 ID1 两组对边为 **16.511～16.513 / 16.594～16.602 mm**； + 留出棋盘角点 RMS 为 **0.188～0.197 px**。 +- 当前内参去畸变后为 **16.521～16.523 / 16.636～16.645 mm**，相较原始像素仅变化约 + 0.01 / 0.04 mm，不能解释相对 16 mm 的约 0.5 mm 差异。 +- 另用传统棋盘角点检测加亚像素细化，并分别使用全部角点、靠近 Tag 的三行或两行 + 拟合平面变换；不同检测方法和网格子集的边长变化小于 0.1 mm，比例差异仍存在。 + 这是同一图像上的方法敏感性检查，不是新增独立实物样本，也不是新的合格标定。 + +结果见各帧的 `coplanar_scale_assessment.json` 及汇总目录中的 +`central_coplanar_sensitivity.json`。上述长度全部以 27 mm 棋盘间距、平面棋盘及 +Tag 与棋盘共面为条件；图像本身不能无条件证明 Tag 的绝对毫米尺寸或两者物理共面。 +这项检查未使用外参、URDF 或关节拟合，不能通过增加关节拟合参数解决其比例矛盾。 + +用户随后确认 16 mm 和 27 mm 均为打印后实物测量值。保留这两个生产输入，不能将 +图像条件换算直接盖过实测值并断定打印错误。已请求测量工具、Tag 黑框宽高及棋盘连续 +五格的未取整总长度，以区分测量精度与几何假设的问题。此次仅进行静止图像与离线 +检查，没有新增整手运动，没有修改相机、尺寸或验收门限，没有生成合格发布 manifest。 + +## 第十七轮评估：依据实物复测修正 Tag 尺寸 + +用户再次量取后明确：“tag 是 16.5×16.5 mm，棋盘还是 27×27 mm”。这提供了独立的 +实物输入修正依据，与先前双目约 16.49 mm、中央共面约 16.51×16.60 mm 基本一致。 +不是从 URDF 拟合残差反推并选择一个易于通过的尺寸。 + +原先按 16 mm 计算 16.5 mm 的方形 Tag，会在同一单目姿态解下将平移距离缩小到 +16/16.5，约低估 3.03%；跨相机变换中的基线平移来自 27 mm 棋盘，并不会随之缩放。 +这构成明确的输入尺度不一致，不能通过增加运动曲线参数来合理消除。 + +修改范围仅为当前 O6 的以下配置及指纹： + +- `profiles/o6_right_8.yaml`：8 个 Tag 均显式声明 `size_m: 0.0165`。 +- `o6_right_8_tags.yaml`：检测器默认尺寸和每个 ID 的尺寸均为 0.0165 m。 +- `o6_three_camera_calibration.yaml`：标定节点尺寸及全部 ID 覆盖值同步为 0.0165 m。 +- `o6_right_product.yaml`：更新以上三个受保护文件的 SHA-256。 + +程序已有 Profile、检测器和标定节点的尺寸一致性检查，本次无需增加型号特判或 +修改拟合算法。其他型号/打印批次没有新的实测依据，不自动替换其配置。棋盘尺寸未变， +没有因此修改相机内外参、源 URDF、旧采集文件或验收门限。非线性关系仍由完整 JSON +和修正 URDF 配合表达。改前配置与改后指纹见 +`20260915_213337/review/whole_chain/tag_size_correction_20260916/`。 + +28 项配置加载、实际 launch 参数和 PnP 参数测试通过；已重新构建安装，核实四个 +配置文件与源码逐字节一致，`--validate-only` 通过。用户移开棋盘及备用 ID1 并确认 +O6 可运动后，启动新会话 `20260916_105528`，显式 `--no-resume` 完整采集。 +最终是否合格须以该会话的独立验收及发布文件为准，不能仅凭尺寸改正确就声明成功。 + +### 新会话结果与收尾内存问题 + +36 个采集单元全部通过，小指第三轮 increasing 方向第一次存在反馈分箱空白 42， +按原最大空白 16 的门限自动重扫一次后通过。完整采集约 6 分 47 秒；保存 15,167 个 +原始图像记录、6,828 个关节样本及 15,629 个 SDK 反馈样本。11:02:20 完成最后采集单元， +随后开始安全回位与收尾。11:05:28 系统内核明确记录 OOM、4 GiB swap 耗尽，终止了 +一个 VS Code 进程；三相机同时出现长时间时钟读取中断，SDK 反馈超时，终端随后发生 +BrokenPipeError。不能把该现象归因于机械手硬件，也不能将此会话记为在线无暂停完成。 + +原始日志完整保留，正式 `--offline-raw` 流程验证了配置指纹、全部采集单元、运动来源、 +独立第四轮与稳态验证数据，生成 `offline_165mm/` 下的 JSON、URDF、报告和 +`release_manifest.json`，退出码 0。JSON/URDF 的 SHA-256 与清单相同；已从保存 JSON +再独立调用重建 CLI，得到逐字节相同的 URDF。原来的角度 MAE/P95/max 1°/2°/3°、 +位置 P95 3 mm 及对应图像门限没有修改。 + +同时发现 `read_journal_prefix` 先整文件读取、再 `splitlines()`，在解码对象之外还保留 +两份原始文本。这份日志为 318,772,511 字节，额外副本会显著增加相机仍运行时的收尾 +峰值。已改为按冻结字节边界逐行解析，保留全部记录、顺序和不可越过边界的校验； +不通过丢数据或修改安全超时规避问题。新增 16 MiB 日志的临时缓冲内存上限回归检查， +并验证短文件与非对象记录拒绝；独立进程、取消、真实 finalizer 等共 9 项测试通过。 +已重新构建安装，进一步用同一实采数据检查独立收尾进程与已验收文件的一致性。 + +修改后的已安装独立进程以 `require_motion_evidence=True` 处理同一份完整原始数据， +约 **55.21 秒**完成，子进程峰值 RSS 为 **1,344,164 KiB（约 1.28 GiB）**； +JSON、URDF 两个文件的 SHA-256 均与正式离线验收版本相同。该检查没有启动相机或 SDK， +也没有重新运动机械手，不能单凭它宣称修复后在线全过程已经完成第二次实机验证。 +报告为 `20260916_105528/isolated_finalization_verification.json`。 + +通过已验收文件的字节/哈希与 JSON→URDF 对应关系复核后，使用现有发布器将相同文件 +发布到产品根目录的 `20260916_105528_verified_165mm/`，并更新 `latest_partial_passed`。 +原始中断会话、离线验收目录与独立进程检查目录全部保留；没有把原始 PAUSED 状态改为 +COMPLETE。正式目录中的 `release_origin.json` 记录已验收来源；完整结论和实际采集范围 +见 `20260916_105528/verification_summary.json`。当前布局直接测量拇指和小指共 5 个关节， +食指、中指、无名指共 6 个关节的运动曲线按原配置从小指迁移，不宣称这些关节独立实测。 + +## 第十八轮评估：被动关节由 JSON 提供角度，消除线性导出冲突 + +用户指出拇指 IP 在修正 URDF 中明显弯得更小。复核确认，原 URDF 的线性倍率为 1.86， +旧修正产物的倍率为 1.16556243135602，而实测 JSON 的 IP/pitch 比例随行程约从 1.71 +变化到 1.94。父子下限分别为 -0.00172483145889436、-0.0020103987491093 rad， +旧“共同零点＋整段不越界”约束把这两个小负端点的比值变成全行程倍率上限。 +这不是非线性 JSON 的拟合失败，而是将独立实测曲线再次约束为一条直线的导出设计问题。 +旧完整 JSON＋自定义 FK 验收虽通过，标准 ROS mimic 仍会覆盖被动角，因此显示与验收不一致。 + +评估后采用统一的 JSON 角度来源规则，不修补 O6 的某个倍率： + +- 新 v3 修正 URDF 移除原被动关节的 ``，全部关节角由 JSON 明确给出。 + 父子链、转轴、连杆、零位与限位保持原有受检语义；原被动关节没有新增实机驱动通道。 +- `urdf_correction` 升为版本 2，保存 `joint_angle_source: calibration_json`、 + `passive_joint_sources` 和明确的删除操作。只有完整覆盖运动关节、来源与原 URDF 一致、 + 被动关节与来源关节共享 SDK 通道时，才能授权移除；其他数值编辑仍使用 Profile 原授权。 + 导出版本与采集配置分开，未修改 Profile 指纹或原始记录来绕过保护。 +- JSON 先保存，重新读取后重建 URDF；普通 FK、角度/位置验证及原始图像验收使用该最终文件。 + 标准 `robot_state_publisher` 可直接接收全部关节状态，不需要忽略 mimic 的定制算法。 +- 线性拟合仅保留报告诊断，标记 `diagnostic_only_not_exported`，不再按限位限制拟合倍率， + 不参与导出和运行角度。旧 correction v1 的精确重建和旧读取方式继续兼容。 + +专项回归覆盖微小负限位、非线性行程、普通 FK、JSON 重建字节一致性、错通道、缺失曲线、 +非法来源、未经授权删除与拓扑改动拒绝。真实 ROS TF 测试在独立命名空间验证三个弯曲点， +未连接 SDK；末节角度不再被覆盖。O6/G20/L6/O12 的合成完整收尾、独立图像验收、迁移来源、 +双向映射及旧格式检查通过；这不是其他三型号的新增实机精度证明。 + +已构建安装，并用 `20260916_105528/raw_samples.jsonl` 的完整 16.5 mm Tag 实采数据 +执行正式离线回放与发布，退出码 0。新目录为 +`calibration_output/O6_RIGHT_001/20260916_123347_json_driven_165mm/`， +`latest_partial_passed` 已更新。原来的角度、位置和原始图像门限全部通过，没有放宽。 + +对新旧发布文件逐项比较：11 个关节的全部指令表、反馈映射、零位和限位不变； +URDF 的结构差异仅为移除 5 个线性 mimic。重新读取新 JSON 重建得到逐字节相同的 URDF。 +相同输入的拇指结果如下（单位：度）： + +| SDK 指令 | CMC pitch | 旧 URDF 线性 IP | 新 JSON＋URDF IP | +| --- | ---: | ---: | ---: | +| 192 | 9.3781 | 10.9307 | 16.0523 | +| 128 | 18.8914 | 22.0191 | 33.0907 | +| 64 | 27.2887 | 31.8067 | 49.6742 | +| 0 | 34.2391 | 39.9078 | 66.3515 | + +复核脚本与结果保存在原会话的 `verify_json_driven_export.py`、 +`json_driven_export_verification.json`，正式回放日志为 `json_driven_replay.log`。 +旧发布三文件的哈希未变。本次没有重新运动机械手，沿用原 5 个独立观测关节、6 个迁移关节 +及 CAD 零位假设的精度边界。使用新产物时须通过 JSON 同时驱动全部关节；单独拖动 URDF +父关节滑条不再自动联动末节，外部仿真也须移除与 JSON 冲突的旧线性更新逻辑。 + +## 第十九轮评估：按用户要求保留标准 mimic 近似联动 + +用户明确使用 `https://viewer.robotsfan.com/` 直接预览 URDF，并进一步说明： +非线性关节也必须保留 `` 自动联动，单独 URDF 尽量接近实机； +需要实测非线性结果时再由标定 JSON 查找表控制。上一轮删除 mimic 的正式导出方向 +不符合这一使用要求。已核对网站当前构建及官方 URDFAdapter：滑条直接设置 URDF 关节， +没有本项目标定 JSON 的读取逻辑，不能期望它自动获得 JSON 中的非线性曲线。 + +### 方案评估与实现 + +标准 mimic 使用 `q_child = a*q_parent+b`。将 `b` 强制为 0，再约束父子全行程， +使拇指的两个微小负端点决定全局倍率上限，这是旧版 1.16556 倍率失真的原因。 +仅恢复 ``、手填倍率或扩大限位都不能解决模型与约束之间的矛盾。 + +现在使用两端子关节角度作为拟合变量： + +- 父关节输出范围 `[p0,p1]` 保持不变，拟合 `c0=q_child(p0)`、`c1=q_child(p1)`。 +- 两个变量分别受子关节原输出限位约束；区间内的线性插值自然不会越界。 +- 由 `a=(c1-c0)/(p1-p0)`、`b=c0-a*p0` 导出标准 mimic 倍率和偏移。 +- 沿用前三轮成对视觉样本与稳健损失拟合，第四轮只验证；不改变 SDK→角度表、 + 坐标零位、Tag 安装、相机参数或训练支持的限位。 +- `b` 是线性近似在零点的误差，不是新测出的机械零位。报告同时保存 + `baseline_error_rad`、`physical_zero_correction: false` 和实际残差。 + +生产 `urdf_correction` 使用版本 3，并声明 +`mimic_policy: range_bounded_affine_approximation`、`joint_angle_source: calibration_json`。 +仍先保存 JSON，再读取生成 URDF。所有原 mimic 来源与 XML 拓扑保留； +数值修改继续使用已有 Profile 授权，没有更改采集指纹。版本 1、2 产物保留重建兼容， +不原地覆盖。完整 JSON 回放显式使用非线性被动角，URDF 单独回放使用 mimic； +两者分别报告精度，线性近似不能冒充 JSON 的实测精度。 + +专项检查覆盖微小负限位不压低全程倍率、正负运动方向、全行程限位、留出样本禁止参与拟合、 +重建字节一致性及缺失/降级导出规则拒绝。真实标准 ROS TF 检查只给父关节角度， +验证被动关节按导出 mimic 联动；历史无 mimic 版本的独立关节测试也继续通过。 + +### O6 实采数据复验与发布结果 + +相关 121 项检查通过，覆盖四型号合成产物、迁移来源、双向表、历史格式及标准 ROS 联动。 +构建安装后，以同一份完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0， +生成并发布 `20260916_124718_mimic_json_165mm/`，更新 `latest_partial_passed`。 +JSON＋URDF 的角度、位置及独立原始图像验收通过，原精度门限不变。 + +新旧三版的全部 11 关节指令/反馈曲线、几何零位、限位逐项一致；5 组 mimic 来源与 +原始 CAD 相同。拇指 IP 倍率为 **1.86604745134131**，偏移为 +**0.00120821859875384 rad（0.06923°）**。小指及三个迁移末节的倍率为 +**0.870364582419037**，偏移为 **0.0348981257618717 rad（1.99952°）**。 +这些偏移是近似关系在零点的残差，完整 JSON 的 baseline 仍为 0。 + +| SDK 指令 | CMC pitch | 旧 URDF IP | 新 mimic IP | JSON IP | +| --- | ---: | ---: | ---: | ---: | +| 255 | 0.0000° | 0.0000° | 0.0692° | 0.0000° | +| 192 | 9.3781° | 10.9307° | 17.5691° | 16.0523° | +| 128 | 18.8914° | 22.0191° | 35.3214° | 33.0907° | +| 64 | 27.2887° | 31.8067° | 50.9912° | 49.6742° | +| 0 | 34.2391° | 39.9078° | 63.9611° | 66.3515° | + +相对 JSON 的 256 项指令网格，拇指线性近似最大差约 **2.3905°**，小指及对应迁移末节 +最大差约 **4.1965°**。这是两个表示之间的差异，不是新增的实机独立精度测量； +独立留出结果分别保存在 manifest 的 `full_json_urdf_holdout` 和 `urdf_mimic_approximation`。 +因此网站中的联动预览仍是近似，需要实测非线性结果时必须使用 JSON。 + +另行读取新 JSON 重建得到逐字节相同的 URDF。所有旧发布文件哈希不变， +没有重新运动机械手，也没有改写旧采集结果。复核脚本、结果及正式回放日志分别为 +原始会话下的 `verify_affine_mimic_export.py`、`affine_mimic_export_verification.json`、 +`affine_mimic_replay.log`。本次仍只独立观测拇指、小指 5 个关节，其他 6 个关节沿用原迁移关系。 + +## 第二十轮评估：全型号保留原始 mimic，实测非线性由 JSON 提供 + +用户确认统一采用原始 mimic 作为基础联动，暂时取消正式标定中的 mimic 参数优化。 +该规则替代第十九轮新产物的拟合策略；历史产物保持原样及原有读取语义。 + +### 全型号依据与边界 + +检查 L6、O6、O12、G20 的左右手原始 URDF，以及当前四套右手产品配置。 +L6/O6 部分曲线从小指迁移，O12 包含两级 mimic 链和无名指曲线迁移，G20 配置覆盖全部实测关节。 +左右手系数不能互相套用：O6 拇指左右分别为 2.29/1.86，G20 为 1.02/1.03。 +原始文件也不等于限位自洽：按原始右手父关节全行程，L6 拇指约超出子限位 3.6795°， +O12 无名指中节约 0.7162°，G20 四个 DIP 约 0.4297°。这些是文件内冲突，不是硬件越限结论。 +O6 的已声明实测范围策略不能自动授予其他型号放宽 CAD 硬限位的权利。 + +### 统一实现 + +- 新 correction schema v4：`mimic_policy=preserve_source_mimic`，`joint_angle_source=calibration_json`。 +- measured 正式拟合及收尾不再调用 mimic 参数优化，原 mimic 来源、倍率、偏移及省略属性全部保留。 +- `source_mimic.py` 统一计算负倍率、非零偏移及串联关系的可达范围，不含型号分支。 +- `mimic_envelopes` 单独记录基础范围、联动可达范围和最小导出包络；不会改写实测范围证据。 +- 包络扩展只适用于已有实测范围策略；默认 CAD 硬限位和独立安全限位继续生效，冲突明确报告关节及范围。 +- 零位仍修正 origin。原始 mimic 只作为修正坐标中的预览近似，精确 FK 使用 JSON 提供的各关节角。 +- 新 JSON 禁止请求 mimic 参数修改或删除；最终验收对照受保护原文件核对 mimic 元素和可信范围证据。 +- correction v1/v2/v3 继续兼容重建与读取，既有原始记录和配置指纹不变。 + +### 验证与 O6 发布 + +两组相关检查分别 **68 项、72 项通过，共 140 项**。覆盖八份左右手源文件、负倍率、零倍率、 +非零偏移、多级链、独立安全界限、非法 JSON 修正、历史版本重建及标准 ROS 的父关节联动。 +四型号合成完整流水线同时验证：禁止调用 mimic 优化函数，仍能完成 JSON→URDF→独立角点验收→发布→读取。 +这些合成检查不代表其他型号已经完成实机标定。 + +构建后以完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，发布： +`20260916_134154_source_mimic_165mm/`，`latest_partial_passed` 指向该目录。 +5 个 mimic 元素与原始 O6 URDF 一致，拇指 1.86、其余四个末节 0.89，offset 全为 0。 +所有 11 关节指令/反馈查找表、几何零位、实测范围证据与前三版逐项一致，旧产物哈希不变。 +相对上一版 URDF，除 mimic 参数外仅 IP 下限改变： +`−0.0020103987491093 → −0.00320818651354352 rad`，约增加 **0.068628°** 的联动兼容范围。 + +SDK 指令 0 时，拇指 pitch 为 34.2391°，原始 mimic 预览 IP 为 63.6848°，JSON IP 为 66.3515°。 +完整 JSON＋URDF 的拇指 IP 独立角度 P95 为 0.43466°、位置 P95 为 2.11692 mm； +小指 DIP 分别为 1.87341°、1.94512 mm，原精度门限未改。 +独立图像验收的门限、样本数量和通过结论保持不变；重复数值优化的末位浮点值及诊断项可能变化。 +读取保存的 JSON 重建得到逐字节相同 URDF；本次没有重新运动机械手。 + +复核材料位于原始会话目录：`verify_source_mimic_export.py`、 +`source_mimic_export_verification.json`、`source_mimic_replay.log`。 +当前 8 Tag 布局仍只独立测量 5 个关节，其余 6 个按既有声明迁移；在线采集曾有 OOM 的边界保持原记录。 diff --git a/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md b/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md index 5183571..6f41fa6 100644 --- a/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md +++ b/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md @@ -1,6 +1,80 @@ # 按关节零位与实测/复制 JSON v3 实施记录 -日期：2026-09-11。范围：`linkerhand_calibration`。 +更新：2026-09-15。范围：`linkerhand_calibration`。 + +## 当前收敛结果 + +- G20、L6、O6、O12 右手共用四轮连续扫描，随后执行独立的稳态指令映射训练与验证。原精度门限保留。 +- 正式入口在启动硬件前检查运动覆盖与绝对零位来源。O12 的 `thumb_mcp`、`pinky_pip`、`ring_pip` + 按用户确认沿用原始 URDF 零位，记录为 `assumed_source_cad_zero`；运动继续实测或按声明复制。 +- 收尾先保存 JSON，再由 `runtime/artifacts/urdf_from_json.py` 读取它重建 URDF。 + 发布器检查修正元数据能否逐字节重建最终文件；可独立使用 `rebuild_calibrated_urdf` 命令。 +- 四型号的合成原始观测均已通过实际拟合、文件生成、验收、发布及读取器加载。 + 合成夹具使用明确的几何真值与范围，这不代表四种实机均完成验证。 +- O6 会话 `20260915_134211` 实采 36/36 单元一次通过，无重扫、无采集中断，采集耗时 412.35 秒。 + 同一原始日志已生成候选 JSON 与修正 URDF，独立 JSON 重建得到相同字节；整链几何验收仍失败，未发布。 + 最大 Tag 位置 P95 为 15.32 mm，小指末节姿态 P95 为 7.76°，不能称为整手精度合格。 +- 随后用棋盘格重新测得 O6 三相机外参：两组各 15 个姿态，最大联合重投影 RMS 为 0.928 px， + 最大重复平移差 0.341 mm、旋转差 0.043°，通过原门限。新配置为 + `config/o6_three_camera_extrinsics_20260915.yaml`；旧数据保留原外参身份，不混用。 +- 新外参会话 `20260915_144514` 完成拇指 24/36 单元后，小指准备求解有两个候选耗尽 150 次迭代。 + 同一批 120 帧离线复现确认是内部 LSMR 线性求解精度不足；调整 `atol/btol` 为 `1e-10` 后， + 四候选均在原 150 次预算内收敛。外层收敛条件、分支统计和物理精度门限未变。 + `test_recorded_pinky_alternatives_converge_within_original_budget` 保存该实采回归；相关 34 项测试通过。 +- 恢复会话 `20260915_145437` 暴露旧日志在运动回调中校验导致的反馈超时报错：离线复现读取约 + 1.41 秒、完整凭据/单元校验约 13.88 秒。`PreparedResume` 将静态校验移到回调启动前； + 当前固定基准与逐关节安装仍在现场独立验证。后续 `20260915_145748` 暴露一次性导入大日志 + 仍会阻塞；改为每个控制周期最多导入 64 条记录，全部持久化后才确认复用。 + 恢复相关 73 项测试通过，包含导入期间持续处理反馈、接收中止及落盘前不得跳过单元；原 1 秒反馈保护未变。 +- 图像模型零位恢复还需同步导入该零位的 `pnp_candidate_frame`，不能只复制 `joint_zero_sample`。 + 已修复该来源闭合问题；真实中断恢复会话离线核对发现恰好漏掉 10 帧零位图像，按新导入逻辑保留后 + 原凭据检查通过。原始日志未修改。相关 74 项测试通过，包含连续两次恢复的零位图像回放。 +- 新外参完整会话 `20260915_150015` 已完成 36/36 单元，采集中无暂停，两次自动重扫均通过。 + 生成的候选 JSON 可独立重建出字节相同的修正 URDF，并通过标准 ROS URDF 加载。 + 整链训练阶段的刚性 Tag 安装检查仍失败：拇指 IP 位置 P95 为 9.73 mm，小指 DIP 姿态 P95 为 9.40°。 + 这不是第四轮整链精度通过的结果，未生成发布 manifest，原始 URDF 未覆盖。 + 用户已确认 ID4/ID5 安装在平整硬片上，Tag 黑框为 16×16 mm，棋盘格单格为 27 mm。 + 尚不能据这些残差判定硬件故障。 +- 随后完成三相机原始棋盘格内参核验：正面 34、侧面 25、顶部 24 个不同姿态； + 每第 4 张预先留作独立验证。验证重投影 RMS 从旧内参的 0.880/0.955/0.484 px， + 降至 0.276/0.465/0.317 px。新内参保存在 `config/o6_camera_intrinsics_20260915/`。 + 配套外参使用正面/侧面 25 组同步观测和补采的正面/顶部 15 组同步观测，均无剔除； + 最大 RMS 0.575 px、旋转重复差 0.024°、平移重复差 0.269 mm，通过原门限。 + O6 产品配置已指向 `config/o6_three_camera_extrinsics_intrinsics_20260915.yaml`， + 并通过无运动的配置及受保护输入校验。旧相机文件与旧整手记录保留原身份。 + 原图、角点、训练/验证划分、拟合报告及导出哈希保存在 + `calibration_output/O6_RIGHT_001/camera_intrinsics_audit_20260915/`。 + 相机核验通过后仍须新采整手数据，不能将旧整手会话改挂新相机参数发布。 +- 新相机完整重采 `20260915_154743` 的拇指 24/36 单元全部完成，无重扫；小指准备的四候选 + 均在 26–36 次迭代内收敛，但候选区分证据不足，停在 `image_motion_families_not_distinguishable`。 + 随后发现统一 launch 将四型号受保护 Tag 配置中的 `detector.decimate=1.0` 强制覆盖为 `1.5`。 + 已移除这层未声明在产品配置内的覆盖，检测设置统一来自受保护 YAML；不修改候选区分或物理精度门限。 + 新回归实际展开 ROS 组件参数，修复前四型号均复现 `1.5 != 1.0`；修复后启动及 runner 的 55 项测试通过。 + 构建和 `git diff --check` 通过；旧降采样记录不混入新结果。 +- 原分辨率完整重采 `20260915_160040` 已从三台运行节点读回 `detector.decimate=1.0`。 + 拇指 24/36 单元一次通过，无重扫；小指准备的 104 帧、四候选均在 21–35 次迭代内收敛， + 仍因 `image_motion_families_not_distinguishable` 暂停，未生成本次完整候选 JSON/URDF。 + 离线回放中两个未分开的 ID4 姿态旋转差 P95 为 18.64°，不能当作同一解； + ID4 法线与相机光轴夹角在两解下分别约 6–8°、10–11°，运动中一直接近正视。 + 已请求保持相机、手掌和 ID3 不动，将 ID4 硬片垫斜后重新采集；这不是硬件故障结论。 + `review/all_view_detections/` 保留本次后 200.7 秒的三机位检测与 SDK 消息。 + 其中侧摆期间同时看到 ID1/ID2 的匹配样本仅 9 帧、反馈范围 206–255， + 不足以判断整个侧摆行程是否引起被动关节联动。所有精度门限保持原值。 +- 上述修复后的四型号文件链、JSON 重建、恢复凭据和图像求解定向回归共 55 项通过（76.55 秒）， + 构建通过，`git diff --check` 通过；142 个原始 URDF/mesh 文件仍与 HEAD 相同。 + +本次生成测试见 `test_all_model_artifact_pipeline.py` 和 `test_json_urdf_rebuild.py`。 +O6 最新采集及暂停证据在工作区 `calibration_output/O6_RIGHT_001/20260915_160040/review/`； +最近一次完成采集并生成候选文件的证据在 `calibration_output/O6_RIGHT_001/20260915_150015/review/`； +旧外参会话证据保留在 `calibration_output/O6_RIGHT_001/20260915_134211/review/`。 +下方分批测试数字为各次改造的历史记录。 + +2026-09-15 最后一次全包回归覆盖 1424 项：1408 项直接通过、15 项旧夹具兼容问题、1 项缺少实测记录跳过。 +这些失败项已修复，按失败清单重跑 15/15 通过，没有遗留失败；这是全量运行加失败项复测的结果。 +O12 使用当前 CAD 假设的完整生成/验收测试另行通过。构建与四型号安装入口检查通过， +142 个原始 URDF/mesh 文件与 HEAD 内容一致。 +完整日志和复测日志保存在上述 O6 `review/` 目录的 `linkerhand_final_full_tests.log`、 +`linkerhand_final_rechecks.log`；`software_validation.json` 汇总了验证范围。 ## 实现与坐标约定 @@ -38,16 +112,23 @@ Profile 必须声明完整零位指令和非零的最终到达动作；扫描起 JSON 关节来自自己的视觉样本，或 Profile 明确声明的对应实测来源；同一 SDK 通道允许绑定多个关节。 接收关节保留自己的 SDK 通道，复制来源的指令/反馈节点及角度，不再拟合。 接收关节没有独立零位帧和第四轮指标，报告标记 `independently_measured=false` 并引用来源证据。 -正式单条指令曲线只用前三轮，约束 baseline=0；两条方向诊断曲线保留回差，共用同一参考。 -第四轮分别检验两方向，不能用于重新拟合曲线、零位、Tag 安装或基座。 +运动曲线只用前三轮，第四轮分别验证两方向；独立稳态分区用于指令映射训练与验证，约束 baseline=0。 +O6 侧摆按已确认策略输出正反向指令曲线；其他关节沿用单值指令曲线。两方向共用同一参考。 +验证数据不能用于重新拟合曲线、零位、Tag 安装或基座。 序列化只导出冻结结果；u8 必须实测支持 0–255 才能生成 256 项，rad 只在 `input_values` 范围内插值。 -URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出 offset 精确为 0。 -非线性 JSON 与 mimic 近似分别保存。近似误差不否决完整映射；机械范围、拓扑、授权字段及标准加载仍必须合法。 +修正 URDF 保留各型号、本侧的原始 mimic，单独使用时提供近似联动；完整非线性角度由配套 JSON 提供。 +`urdf_correction` v4 明确 `preserve_source_mimic`，正式标定不再拟合倍率或偏移， +不复制其他手指的 mimic 参数，也不因修正 origin 而重写原 mimic offset。准确 FK 显式使用 JSON 的各关节角。 +`mimic_envelopes` 与实测 `motion_range_evidence` 分开：只在明确声明的实测范围策略下， +取实测范围与原始联动可达范围的最小包络；禁止突破独立安全界限及默认 CAD 硬界限。 +多级 mimic 使用父关节真正的联动可达范围递推，不将其更宽的 JSON 运动范围继续放大。 +JSON 重建与最终文件验收均核对原 mimic 元素、最小包络、通道一致性及非线性表。 +旧 correction v1、移除 mimic 的 v2、拟合近似的 v3 保持原有重建/读取语义。 既无运动观测也无显式复制声明，或缺少绝对零位来源时，保留相对运动诊断并拒绝完整发布。 旧 mimic 及旧迁移字段不能自动授权 v3 复制，不用默认零偏补全未知几何。 接受原 CAD 末节零位的前提单独记录在 `cad_zero_assumptions`，不作为独立实测证据； -仅这些明确列出的末节取 δ=0，保持其自身 `origin`。O12 的被动 PIP 不在该假设范围。 +仅明确列出的关节取 δ=0，保持其自身 `origin`；O12 的三个额外关节假设见上方当前结果。 ## 验收与消费 @@ -55,7 +136,7 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出 1. 最终 JSON 对第四轮原始视觉角度；逐实测关节、逐方向检查；复制表另核验来源与一致性。 2. 最终 JSON＋修正 URDF 的完整 FK 对第四轮 Tag 位姿；显式使用所有被动角。 -3. 相同主动映射配合标准 mimic 的损失；使用同一份训练冻结安装关系，只报告近似误差。 +3. 相同主动映射配合标准 mimic 的独立近似损失，单独报告。 前两项精度门限仍为角度 MAE ≤1°、P95 ≤2°、最大 ≤3°，Tag 位置 P95 ≤3 mm。 三文件哈希、保护输入、观测覆盖和通道绑定一并检查；拟合失败不自动运动，取消后不能提交。 @@ -63,7 +144,8 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出 明确记录观测关节、复制关节和 CAD 零位假设。未观测接收关节不具有独立精度结论。 公共读取器和 `calibrated_joint_state_bridge` 支持 v3 的完整关节映射及历史 v1/v2 原语义。 -完整 FK 使用 `independent_mimic_angles=True`。标准 mimic 加载器用于结构兼容检查，不能覆盖完整非线性关节状态。 +完整 JSON 回放使用 `independent_mimic_angles=True`，防止线性近似覆盖实测被动角。 +标准 `robot_state_publisher` 和普通 URDF 网页查看器提供线性联动预览，不据此宣称实测非线性精度。 本轮未修改 MuJoCo、相机时间同步和接触动力学；现有 MuJoCo 脚本尚不能视为支持 v3。 ## 当前型号的发布边界 @@ -73,12 +155,13 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出 | G20 | 21 / 21 个运动关节 | 无 | 五个末节的原 CAD 零位为明确假设 | | L6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | MCP 零偏明确迁移；五个末节 CAD 零位为假设 | | O6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | 同上 | -| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 零位仍须另行确定 | +| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 及末节沿用已确认的 CAD 零位假设 | `--validate-only` 的 `independent_measurement_coverage` 返回具体关节列表。 配置检查通过表示合同可执行，不能解释为全关节精度通过。 上述运动策略已经覆盖各型号全部运动关节；完整发布仍要求实测来源验收通过、零位有来源且范围合法。 -O12 当前仍被三项几何缺口阻止完整发布；L6/O12/G20 的现有 CAD mimic/限位冲突保留诊断，不能放宽限位掩盖。 +四个产品配置的零位来源均已明确；L6/O12/G20 的原 CAD mimic/限位冲突保留诊断， +收尾保留限位约束内的线性近似；所有 JSON 查表角度和 URDF mimic 全行程都必须处于合法范围内。 现有 Tag 布局没有被补写成未经验证的新布局，新增参考动作也没有标为实机已验证。 ## 原按关节零位改造的软件验证记录 diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 732af2f..d76e324 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -4,7 +4,7 @@ G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器 验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。 本包按观测关节冻结实机 baseline 零位，输出实测曲线及明确声明的复制曲线组成的 JSON v3， -以及线性 mimic 近似 URDF；报告区分独立实测、迁移和 CAD 零位假设。 +以及保留线性 mimic 联动的修正 URDF；报告区分独立实测、迁移和 CAD 零位假设。 保留 CAD 连杆尺寸、轴位置、mesh、惯量。完整运动由 JSON 的全部关节角度配合修正 URDF 计算。 软件仿真通过不等于实机精度通过；当前原始 G20/L6/O12 存在需要确认的 mimic/限位冲突。 本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。 @@ -27,10 +27,43 @@ ros2 run linkerhand_calibration calibrate_hand --config \ ``` 其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。 +AprilTag 检测参数直接读取产品指定的受保护 Tag YAML，统一启动文件不再覆盖其中的采样分辨率。 `calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。 `--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。 重新开始时先安全回基准，仍必须确保现场没有障碍物。 +### JSON 到修正 URDF + +统一收尾顺序为：拟合与独立角度验证 → 保存标定 JSON → 重新读取 JSON 修正原始 URDF → 文件回读与整链验证 → 发布。 +JSON 的 `urdf_correction` 保存原始 URDF SHA256 和必要的关节字段修正，重建不依赖内存拟合对象。 +原始 URDF 不被覆盖；关节拓扑、字段授权、完整 mimic 范围和 JSON 查表范围在文件安装前检查。 +新 v3 使用 `urdf_correction.schema_version: 4`、`joint_angle_source: calibration_json`， +并记录 `mimic_policy: preserve_source_mimic`：所有型号保留本侧原始 `` 元素， +不改来源、倍率、偏移或省略的默认属性；正式标定不再拟合 mimic 参数。JSON 保留各关节 +非线性曲线及方向分支，精度验收使用 JSON 提供的全部关节角。父子链、轴和连杆不变， +被动关节与驱动关节仍绑定同一 SDK 通道，所有数值编辑仍按 Profile 授权。 +`mimic_envelopes` 单独记录基础范围、原始联动可达范围和导出范围；在明确的实测范围策略下， +导出范围取二者的最小包络，实测范围证据与查找表不变。未声明该策略的 CAD 限位，以及 +另行声明的硬件安全限位，仍是硬边界；冲突时报告具体关节和所需范围，不压低倍率或放宽容差。 +关节 origin 按标定零位修正后，原始 mimic 作为输出坐标中的近似预览，不作为准确物理约束。 +旧 correction v1、移除 mimic 的 v2、拟合线性近似的 v3 产物继续兼容读取与重建。 +导出规则单独版本化，不改变采集配置指纹或旧原始记录，已发布产物不能原地覆盖。 + +已生成的 JSON 可单独重建对应 URDF，无需相机或 SDK： + +```bash +ros2 run linkerhand_calibration rebuild_calibrated_urdf \ + --config src/linkerhand_calibration/config/o6_right_product.yaml \ + --calibration-json /path/to/calibration.json \ + --output /path/to/new_corrected.urdf +``` + +输出路径必须尚不存在。没有 `urdf_correction` 的历史 JSON 不会被猜测或自动补全。 +同一 JSON 和源 URDF 可逐字节重建相同修正文件；发布器会核对此项一致性，读取器要求相应验收记录。 +历史无修正元数据的已发布文件继续沿用原有读取规则。 +完整文件验收失败时保留候选 JSON／URDF 和诊断，便于复核；只有 `release_manifest.json` +和成功发布指针才代表通过，运行时读取器拒绝把未发布候选当作合格标定。 + | Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 | | --- | ---: | ---: | ---: | --- | | G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v3，256 项指令查表 | @@ -51,17 +84,179 @@ ros2 run linkerhand_calibration calibrate_hand --config \ 复制参数不等于接收关节独立实测；接收关节仍须有明确的绝对零位来源及合法机械范围。 `zero.cad_zero_assumptions` 显式记录“实机 baseline 与原 CAD 末节零位一致”的使用前提， 相应 DIP（及拇指末节 IP）δ=0、自身 `origin` 不改。这是假设，不是视觉测得的绝对 CAD 朝向。 -**O12 的 PIP 不采用此假设。** 当前 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 仍缺绝对零位依据， -可以采集诊断数据，但这些缺口解决前不能发布完整 O12 v3 JSON/URDF。 +按用户确认，O12 的 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 也采用原始 URDF 零位作为 baseline 假设。 +运动曲线仍按实际观测／明确复制关系生成，报告标记 `assumed_source_cad_zero`，不称为独立实测零偏。 +正式入口在驱动硬件前检查运动覆盖及零位来源，缺失依据时直接指出缺项，不采完才发现无法发布。 启动检查分别列出 `unmeasured_joints`（未独立观测）、`unresolved_motion_joints`（也未声明复制） 及 `missing_baseline_geometry`。旧迁移字段单独存在不会自动授权新的 v3 曲线复制。 本次沿用现有 Tag 和避让布局声明延后零位动作，没有把新增动作或缺失 Tag 布局标为实机验证通过。 -每任务四个往返周期：前三轮训练，第四轮独立验证。 -每个方向先连续扫描，再单通道访问稳态点。训练每方向默认 9 点，并包含该通道 baseline，第四轮为交错点加端点； +每任务的运动模型采集四个往返，共八趟单程：前三轮训练，第四轮独立双向验证。 +四个产品配置统一使用 `command_capture_mode: separate`：这八趟均为完整连续扫描，随后另做一组稳态映射训练往返和一组独立验证往返。 +主扫描不逐点停顿；稳态点直接执行移动、稳定反馈、新图像采样，不重复等待两次到位。 +稳态训练点由前三轮连续数据生成 `command_sampling_plan`：在原节点网格上寻找最少的 +保留节点，各关节、各方向的预测曲线插值变化均不超过 0.25°；端点、基准、中间支撑点及 +显式增加的非线性节点保留。缺少完整训练数据或曲线噪声过大时使用原网格。 +这份计划只决定停在哪里，每个保留点的静态角度仍由新图像独立测量，不用连续运动角度 +冒充静态角度。独立验证网格保持原样，不参与节点选择。计划及训练来源哈希随原始记录保存， +拟合、离线回放和断点恢复均重新核验；缺点、篡改或最终误差超限仍禁止发布。 +上一段已经完成到位/稳定检查且下一准备目标完全相同时，可复用一秒内、反馈未变化的 +完整保持姿态，省去重复准备等待；有实际位移、反馈变化或记录过期时执行原准备流程。 +原训练网格每方向通常为 9 点，并保留行程内的 baseline；O6 小指额外包含 239，共 10 点。 +新流程在这份网格上按上述规则精简训练停点。 +验证使用交错点加端点（O6 小指包含 247），两类数据不混用。 +日志中的运动轮编号为 0–3，独立指令映射训练和验证编号为 4、5；界面分别显示“稳态指令映射训练／独立验证”。 +其他采用 `interleaved` 的配置仍在四轮各方向中穿插稳态点，并使用三轮稳态训练、第四轮稳态验证。 +行进图像用于旋转/几何拟合，停稳后的新图像用于 SDK 指令映射；同一张图像不会复制成两类样本。 每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。 +关节零位冻结另需至少 10 张独立有效图像。`acquisition.joint_zero_timeout_seconds` +单独限定首张合格零位图像后的采集窗口，四个产品配置均为 5 秒；几何求解后的跟踪恢复仍最多等待 2 秒。 +反馈不稳定会清空当前零位候选，但不会重置总采集期限；图像数量、1° 稳定性及分支证据要求不变。 +稳定窗口使用每条新收到的 SDK 反馈，按消息时间戳去重，不再只取控制定时器看到的最新一条。 +定时器延迟不能制造“独立反馈不足”；期间真实运动仍会使窗口失效。 +候选清空时的 `steady_capture_reset` 保存反馈窗口和丢弃样本数量，便于区分抖动、断流和调度问题。 +零位样本的 `capture_timing` 记录接收、历史快照和提交时间，用于检查处理延迟。 +图像数量未齐时先返回缺样结果，不在控制定时器中反复校验全部姿态。 +四轮及稳态采集完成、数据落盘后关闭图像采集；拟合只使用已保存的相机和参考证据。 +之后的新图像和相机消息不再改变拟合输入，也不触发采集暂停；SDK 反馈、故障保护及操作者中止持续生效。 +拟合或文件验收失败结束为 `FAILED`，保存 `failure_diagnostic.json` 中的失败阶段、原始数据路径和异常堆栈。 +这类失败应先离线复算，不能仅因状态失败就重复四轮运动。运动采集期间的固定参考检查保持生效。 + +O6 的顶部 ID7 与正面 ID2 实际固定在同一末节，Profile 将 ID7 明确绑定到 `rh_thumb_distal`。 +`thumb_yaw` 是它承担的侧摆观测角色，不是其安装连杆名称。整链回放包含该 Tag 上游的侧摆、弯曲和末节角度， +保持关节沿用记录的到达方向。新绑定改变了 Profile 哈希，旧会话不会被自动复用为新配置的通过结果。 + +固定 Tag 的 ID 解码成功不代表边框完整。独立的视觉边框检查进程按完全相同的图像时间戳配对整流图像与检测结果， +检查当前检测四边的黑白边界支持；部分遮挡产生的畸变四边形会被记为 `detection_quality_rejected`， +不能进入 PnP、关节样本或固定基准移动计数。配对缓存有界，不用邻近时间的图像替代。 +检查使用当前检测四边形，清晰 Tag 发生真实位移仍进入原有 5 px／连续 10 帧保护。 +控制进程只订阅 `detections_checked` 小体积观测；三路大图像的传输、配对和边框处理不占用 SDK 控制节点的回调队列。 +ROS 宿主将准备几何求解和逐帧图像角度求解交给独立计算进程，避免与 SDK 反馈争用 Python 执行资源。 +准备请求仍限时 120 秒；逐帧请求最多三个在途，超时或工作进程不可用的图像不生成有效样本。 +工作进程只接收不可变图像/模型输入，没有 ROS 或 SDK 控制接口。返回后仍检查当前图像、跟踪版本和会话版本， +不能将迟到结果提交到下一运动段。`motion_solver_diagnostic.jsonl` 记录请求、计算耗时、线程 CPU 时间及提交结果。 +当前任务需要的固定 Tag 无有效观测时，仍按原采样规则等待或暂停；其他机位的遮挡不误报为移动。 稳态点缺失与连续扫描不足统一在方向结束处理，最多同速重扫一次。 +首次关节零位准备、避让及收尾属于独立的必要动作，不计入四个正式往返；不会每轮重新锁定零位。 +分段加减速和停点仍有耗时，八趟行程不等于耗时恰好降为原来的三分之一。 +新产品会话记录 `capture_schedule_version=unified_schedule_v3_separate_mapping`；旧 `interleaved` 配置仍记录 v2。 +采集模式或保护配置不同的旧断点不与新流程拼接， +历史原始数据仍保留原语义供对应配置离线读取和分析。训练／验证身份与精度阈值保持不变。 字节拟合不再改写原始 command/feedback，也不将接近端点的读数伪装成 0/255。 +## O6 右手拇指短程诊断 + +当前 O6 实物在 2026-09-16 复测的 Tag 黑色码区边长为 **16.5 mm**，棋盘格边长仍为 +**27 mm**。O6 Profile、检测器配置和标定节点配置已同步采用 `0.0165 m`，产品配置记录 +对应文件指纹。该数值属于这套实物标记，不是所有型号或所有打印批次的统一尺寸。 +更换标记时应实测黑框，并同步维护上述配置；旧 16 mm 会话保留原尺寸记录，不作为 +新尺寸的恢复断点。Tag 尺寸修正不改变 JSON 中非线性运动曲线与修正 URDF 的配合方式。 + +本套 O6 的 2026-09-16 实采数据已生成通过验收的产物，目录为 +`calibration_output/O6_RIGHT_001/20260916_134154_source_mimic_165mm/`， +`latest_partial_passed` 已指向该目录。JSON 和修正 URDF 应配套使用；当前 8 Tag 布局 +直接测量拇指、小指，其余三指沿用配置中的曲线迁移关系。采集完成后的在线收尾曾发生 +系统 OOM，最终产物由完整原始数据的正式离线流程验收；日志读取内存优化另经独立进程 +验证，不能将此记录描述为在线全过程无中断。详细证据见 `CALIBRATION_PIPELINE_REVIEW.md`。 +此目录用同一份原始数据重新验收，5 组 mimic 的完整元素与原始 URDF 一致： +拇指倍率 **1.86**、其他四个末节 **0.89**，偏移均为 **0**。 +11 个关节的指令/反馈查找表及几何零位保持不变；仅拇指 IP 的 URDF 下限从 +−0.115187° 调整到 −0.183816°，覆盖原始联动所需范围，实测范围证据仍为原值。 +拇指 SDK 指令 0 时，IP 的 URDF 预览为 **63.68°**，JSON 为 **66.35°**； +这是线性预览与实测非线性的区别。完整 JSON＋URDF 和独立原始图像继续满足原精度门限。 +`20260916_123347_json_driven_165mm/`（移除 mimic）及 +`20260916_124718_mimic_json_165mm/`（拟合倍率和偏移）为历史版本，已被本次发布替代。 +旧目录均保留；始终使用同一 manifest 指定的 JSON、报告和 URDF。 + +定位 Tag 姿态跳变时，可沿用相同产品配置添加诊断选项： + +```bash +ros2 run linkerhand_calibration calibrate_hand \ + --config src/linkerhand_calibration/config/o6_right_product.yaml \ + --diagnostic-thumb +``` + +诊断选择原 Profile 中的 `thumb_yaw_top` 和 `thumb_pitch_ip_front`，各采一次往返， +观测拇指 CMC yaw、CMC pitch、IP 三个关节。保留原速度、完整扫描行程、准备/避让、 +固定基准锁定、按关节尝试零位确认和安全收尾；其余手指不执行扫描任务，但开始时仍会安全回到 baseline。 +本次诊断只启动正面和顶部机位：开始时一次锁定正面 ID0、顶部 ID6；运动观测使用顶部 ID6/ID7、正面 ID0/ID1/ID2。 +侧面不参与本次诊断，ID3 缺失或双解、侧面相机未连接均不会阻塞；因此无需为拇指诊断调整 ID3 或改动相机外参。 +范围由本次全部任务的主/辅观测依赖统一确定，启动、ROS 订阅、设备检查、基准锁定、指纹和进度使用同一范围。 +它不会随当前任务切换而重新锁定；正面和顶部的配置变化、固定基准漂移及所需 Tag 数据不足仍按原策略处理。 +正式全手标定继续使用 Profile 声明的全部机位和固定基准，外参文件及保护输入的完整校验保持不变。 +“短程”缩短的是任务和轮次数，不截断需要复现异常的运动行程。 + +本模式跳过稳态映射采样与三轮训练/第四轮验收，收尾后显示 `DIAGNOSTIC_COMPLETE` +并正常退出。诊断按当前运动 Tag 的原始图像覆盖推进，持续姿态歧义不阻断原始候选采集； +缺少当前运动 Tag 角点或同步反馈导致方向覆盖不足时，仍最多同速重扫一次，严重故障和中止行为不变。 +归零准备证据为 `unresolved` 时，仅诊断模式允许继续收集原始观测：必须先到达 Profile 声明的零位指令， +SDK 反馈稳定，当前运动 Tag 具有足量、带会话和运动版本的图像，才保存独立的 `diagnostic_joint_zero_attempt`。 +它表示访问并观察了声明姿态，不表示已建立关节零位；不写入 `zero_references`，方向切换及重扫不重新归零。 +该组关节后续仅保存原始角点和候选，即使普通跟踪恢复，也不能生成 `joint_sample` 或伪造有效角度。 +缺少零位原始图像、SDK 不稳定、固定基准移动及已冻结参考冲突仍按现有策略暂停；正式标定仍须先确认分支和冻结零位。 +会话目录保存 `raw_samples.jsonl`（含角点和 IPPE 候选）、实际成功冻结的关节零位与 `diagnostic_capture.json`。 +`diagnostic_observation_frame` 在零位尝试和扫描阶段单独记录各帧候选，包括未接受的姿态，不能当作关节角度样本。 +报告 `unit_quality` 分别记录原始观测覆盖与 `accepted_pose_quality` 可信姿态覆盖； +`joint_zero_status`、`missing_joint_zero_references` 列出各关节真实零位状态，`motion_initialization_summary` 保留各候选的关节残差。 +进度显示原始观测覆盖，并单列可信姿态数量和未确认关节；暂停时保持“已暂停”，不会被零位动作名称覆盖。 +可信姿态不足会在退出时明确提示。相邻帧姿态跳变和图像身份用于定位问题，诊断完成不代表标定精度通过。 +每方向的 `observation_timing` 分别统计原始/可见/同步图像数、缺命令与缺反馈图像数、最大图像及同步观测间隔； +缺命令与缺反馈可以发生在同一帧，不能相加当作总丢帧数。少于两帧的间隔记为 `null`。 +方向失败时这些信息同时进入暂停诊断。反馈跨度 100% 仅表示两端都有观测，不代表中间没有缺口。 +原始帧的 `timing` 保存回调接收、历史快照及最新命令/反馈时间戳，用于区分消息延迟与状态匹配问题； +接收到快照的时间包含线程调度和锁等待，不能单独解释为写盘耗时。未匹配反馈时同步误差为 `null`，不会显示为 0 ms。 +可额外加 `--record-bag` 保存现有录制话题；仅检查入口与配置可加 `--validate-only`，不会启动硬件。 + +诊断始终完整新采，不复用断点，不拟合或发布标定 JSON/URDF，也不更新发布指针。 +原始会话带有诊断标记，正式离线拟合和断点发现会拒绝将其用作正式标定数据。 +当前选项只开放给 O6 右手；其他型号仍走原来的正式流程。原始 URDF、Profile 和设备配置无需修改。 +诊断任务预设在 `profiles/diagnostics.py` 声明，运行时只选择既有 Profile 任务，复用公共调度与采集组件。 + +诊断采集完成后，runner 先关闭相机和 SDK 进程，再自动运行公共图像几何分析，生成独立的 +`diagnostic_analysis.json` 和可直接阅读的 `diagnostic_analysis.md`。耗时优化不进入实时状态锁或相机回调。 +分析按任务、机位、方向和尝试分别保留原始图像身份，比较所有候选族的三维 PnP 残差、深度误差比例、 +图像域关节链拟合以及隔帧和前后区间验证。验证时几何参数冻结，只从当前图像反解几何角度，不使用 SDK 角度映射。 +图像误差小不等于毫米精度通过，优化收敛也不代表分支唯一；分析结果不能写回跟踪器、零位或正式标定产物。 +未采到的方向和旧会话缺少的归零前证据会明确列出，不拼接不同会话，不使用第四轮重新拟合。 +图像分析失败或被取消时，已完成的采集及原始文件仍保留，不会改标成 SDK 故障。 + +已保存的数据可直接离线复算，无需启动相机或重复运动： + +```bash +ros2 run linkerhand_calibration analyze_calibration_capture \ + --config src/linkerhand_calibration/config/o6_right_product.yaml \ + --raw calibration_output/O6_RIGHT_001/20260914_192637/raw_samples.jsonl \ + --output calibration_output/O6_RIGHT_001/20260914_192637/image_review +``` + +这些修改解决诊断采集被未确认零位阻塞及报告不完整的问题。现有真实 IP 数据仍有分段几何不一致， +正式分支门槛、拟合器、URDF 修正和独立精度验收保持原要求；软件回归不能替代实机精度结论。 + +### O6 右手 yaw 方向差保持诊断 + +需要区分“到达指令后仍在缓慢稳定”和“保持后仍存在方向差”时，使用独立选项： + +```bash +ros2 run linkerhand_calibration calibrate_hand \ + --config src/linkerhand_calibration/config/o6_right_product.yaml \ + --diagnostic-yaw-hold +``` + +此模式仅选择 `thumb_yaw_top`，只启动顶部机位，使用固定基准 ID6 和运动 Tag ID7。 +每次 Start 都重新采集会话基准并尝试确认 yaw 零位，不复用旧会话；同一会话的两个方向共用首次冻结零位， +不会分别归零。未能确认零位时按诊断规则保留原始观测，并明确标记无法生成可信角度。 + +在原速度和行程下执行一次往返。每个方向扫描后先回到该方向的起点，再沿同一方向访问三个保持点： +递减方向为 `255 → 192 → 128 → 64`，递增方向为 `0 → 64 → 128 → 192`。 +每个保持点在指令轨迹完成后至少保持 10 秒，两方向合计六次保持；反馈确认可能使实际等待稍长。 +保持过程继续记录图像角点、候选、实际指令和同步反馈，用于比较到达后的角度随时间变化及双向最终差异。 +保持阶段的原始观测单独标记，不计入连续扫描的覆盖统计；遮挡、设备故障、中止及安全收尾仍沿用公共策略。 + +`--diagnostic-yaw-hold` 与 `--diagnostic-thumb` 互斥，不能同时使用离线拟合、发布或禁用运动选项。 +可加 `--validate-only` 只检查入口与配置，或加 `--record-bag` 留存现有录制话题。 +结果保存在 `raw_samples.jsonl` 和 `diagnostic_capture.json`，记录保持指令及时间； +不拟合或发布标定 JSON/URDF，不形成正式断点，也不算精度验收。 +本诊断用于查清实机稳定时间和方向差，不改变最终单值 `angle_rad` 的验收要求。 + ## 启动前相机检查 `--validate-only` 检查相机配置文件、序列号、内外参指纹和已有外参质量记录，不采集现场图像。 @@ -97,11 +292,26 @@ ros2 run linkerhand_calibration calibrate_hand --config \ - 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。 - 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。 运动 Tag 的刚性和几何一致性仍须通过最终独立验证。 -- 断点版本为 `unified_engine_v5_joint_zero`，不复用旧策略数据。 +- 采集断点版本为 `unified_engine_v8_all_view_images`；另以 `pose_tracking_policy_version=confirmed_branch_v8_training_model_selection` + 记录姿态分支策略。缺少此字段或版本不同的旧断点不能自动或手工恢复到新会话，历史文件保持原样供离线诊断。 首先检查配置哈希与整场固定参考；各关节轮到时，在同一完整保持姿态核对原零位与父子 Tag 位姿 （5 px、2°、5 mm），通过后才导入对应已完成扫描单元，并保留原零位记录。 整场参考不匹配则放弃旧数据；逐关节安装验证失败则暂停，禁止在旧样本上新建零位拼接。 排除原因后用 `--no-resume` 开始新会话。旧会话文件不修改。 + 旧日志的读取、凭据回放与单元质量检查在实时回调启动前完成，不占用运动/反馈回调。 + 现场验证通过后，每个控制周期最多导入 64 条历史记录；全部落盘后才跳过对应单元。 + 导入期间持续处理反馈、固定基准监测和中止，原 1 秒反馈超时保护不变。 + +连续图像、候选和样本按图像批量追加并刷新到系统缓存，避免持有控制状态锁逐条 `fsync` 阻塞反馈和运动定时器。 +固定参考、关节零位、方向完成和暂停记录仍强制同步到磁盘；拟合、报告哈希、发布和关闭前另有同步屏障。 +断点只复用通过且持久化的完整方向，意外断电时未完成方向不能拼入旧数据；同步失败不得发布。 +此存储策略不改变 50 ms 图像/状态配对门限、方向覆盖门限、拟合公式或精度验收条件。 + +O6/L6/G20 标定读取 SDK 的 `*_hand_state_timed` 反馈接口；位置与每个通道的 CAN 接收时间 +一同保存。G20 五指的分帧反馈分别插值到图像时间，重复发布的缓存不能刷新反馈超时或增加 +稳态独立样本。标准 `JointState` 仍可供其他程序使用，其时间戳保留测量时间。 +`sdk_feedback_sample` 原始记录支持复查时间配对；旧版本只保存发布时间，不能事后补成真实 +CAN 测量时间，也不能与新版断点混用。O12 继续使用独立 HCAN 同步读取接口。 共同坐标约定：`q_CAD = q_output + delta`；每个关节在自身冻结 baseline 时 `q_output=0`。 局部零位通过采样时的完整保持姿态和运动链进入同一掌部参考。固定 Tag 不提供未知末端连杆的 CAD 朝向。 @@ -113,6 +323,8 @@ O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指 不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。 O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。 +连接参数由本包的 `config/o12_sdk.yaml` 保存，并通过产品配置中的 SHA256 校验； +不依赖第三方 SDK 子仓库中未提交的示例 YAML 修改。 只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。 POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。 字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。 @@ -123,6 +335,9 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。 开始后相机投影参数变化也会拒绝继续使用混合坐标数据。 当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag；尚未轮到的手指遮挡不阻塞当前任务。 +零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。 +控制定时器的调度抖动或重复读取同一反馈不会清空稳定观测；真实反馈波动或反馈过期仍会使当前窗口失效。 +零位仍需至少 10 帧且姿态波动不超过 1°；拟合验收门限不变。 短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动 不会实时中断轨迹。无效视觉帧直接丢弃。 @@ -130,15 +345,150 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。 ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 px` 是图像质量拒绝阈值， 不能用它判断两种姿态是否同样可信；仅在误差差值不超过近似同误差阈值时使用时间连续性。 -当另一分支的图像拟合明确更好时允许纠正旧分支。覆盖参数必须有限、非负且小于图像质量拒绝阈值。 -这项修复不保证解决所有平面 Tag 双解，真实角点噪声、采集时序和最终运动精度仍需实测验证。 +分支容差覆盖值必须有限、非负且小于图像质量拒绝阈值。 + +独立 Tag 首次接受姿态前，需要默认至少 5 帧、跨越至少 0.1 秒的图像证据确认。 +近似同误差的候选只有旋转相差不超过 1°、平移相差不超过 1 mm 时才视为同一姿态；这些参数统一定义于 +`core/geometry/tag_pose/parameters.py`，ROS 通过 `pnp_branch_confirmation_frames`、 +`pnp_branch_confirmation_minimum_seconds`、`pnp_branch_equivalence_deg` 加载，离线采集使用相同默认值。 +这只是图像分支的一致性确认，不能把多帧低重投影误差视为物理姿态真值。 + +以下独立图像跟踪规则保留用于预览和历史兼容；新会话的运动 Tag 首次零位还必须通过后述公共运动几何确认。 +参考冻结前，明显更优但与历史姿态发生大跳变的新分支也必须经过确认，过渡帧不进入零位或拟合样本。 +运动 Tag 参考冻结后，若原分支仍有连续、图像质量合格的候选，但另一候选的重投影误差明显更低， +该帧标为 `pose_branch_ambiguous` 并丢弃，内部保留原分支轨迹等待图像证据恢复； +不能仅凭另一候选连续数帧得分更高，就判定冻结参考损坏。不会切换分支、重建零位或把歧义帧送入拟合。 +如果持续不存在符合原分支连续性约束的候选，仍在确认后暂停；长期失联后无法确认分支一致时也会明确拒绝。 +正式标定方向结束仍检查可信样本、覆盖与稳态点，最多同速重扫一次；诊断模式另报原始观测与可信姿态覆盖。 +需要更换分支或重新安装 Tag 时，必须重新开始新会话， +不能把旧冻结参考与新分支数据拼接。原有 0.03 px 容差、35° 跳变、75° 倾角及 1.5 px 重投影质量阈值保持不变。 +真实角点噪声、采集时序和最终运动精度仍须通过独立实测验证。 + +### 首次零位前的公共运动几何确认 + +新生产会话复用 Profile 已声明的**最后一段归零准备运动**，在 `zero_approach` 中保存父子 Tag 的原始候选， +到 `joint_zero` 后先确认分支，再采集原有的稳定零位帧。没有增加扫描轮次、额外试探动作或型号专用求解器。 +几何确认与 SDK 反馈稳定分别检查；几何通过不会提前开启零位采集窗口。 +计算在状态锁外进行，期间持续接收 SDK 反馈并保持声明姿态；计算本身有 120 秒上限。 +几何确认后，先在原有 2 秒观察窗口内等待恢复后的首个有效零位样本，再用 +`joint_zero_timeout_seconds` 限定的独立窗口采齐稳定帧（字段默认 2 秒，四型号产品配置均为 5 秒）。 +窗口不会无限延长。正式采集中，每组尚未冻结零位的关节最多自动恢复一次：可恢复的图像 +准备失败重新执行该组原有的归零准备路径并采集全新图像；几何已经冻结但零位图像不足时， +保持原姿态和模型，只重新开启一次零位图像窗口。原始失败和恢复动作写入 `joint_zero_recovery`。 +已冻结的零位、部分已经提交的多视角几何、保持姿态变化、设备故障不重新拟合；恢复后仍 +不足时明确暂停，保留原有反馈、固定基准保护及用户中止能力。 +中止、暂停或运动版本改变后，迟到的计算结果不能提交。 +前面的避让段、预览、训练扫描和第四轮不参与首次确认。准备段同时核对其他通道的完整保持指令及反馈； +开始后不能改变相机、固定基准或 Tag 安装关系。 + +生产入口 `core/geometry/tag_pose/production_image_motion.py` 枚举 IPPE 候选家族， +使用父子转动副 `R = exp(theta × axis) R_ref`、`t = pivot + R × mount` 拟合原始角点的重投影残差。 +稀疏线性子问题使用 `atol=btol=1e-10`，避免近似计算误差导致外层耗尽迭代预算； +外层收敛条件、默认 150 次迭代预算和各项精度门限保持不变，记录每个候选的求解状态及迭代次数。 +父 Tag 本身运动时联合求解整条已声明的观测链；已冻结的上游几何不会重新拟合。 +角度来自当前图像，不使用 SDK 到角度的先验、旧 mimic 或 CAD 零位假设选择分支。 +单帧 PnP 深度噪声较大时，不再将其三维平移估计直接作为关节运动的真值。 + +当源 URDF 中两个转轴相邻且平行，并且观测 Tag 实际固定在对应连杆上时， +`profiles/observations.py` 提取零位修正后仍不变的平行关系和垂直轴距。 +`CadImageHingeBundle` 消去三个多余几何变量，使准备模型遵守最终 URDF 保留的几何； +不修改原骨长/轴向、不增加权重或 CAD 零位假设。不满足条件的观测链使用原模型。 +新冻结模型保存源 URDF 哈希和约束，最终处理从源文件重新推导核验；旧模型保持可读。 +修改前的原始图像对照实验及适用边界见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。 + +准备段至少需要 24 帧，训练与留出图像身份分离，默认各最多 72 帧。 +训练帧取交错序列，未进入训练的图像才可留出；不因训练帧增加而复用验证图像。 +局部几何可观测性检查计入逐帧角度的不确定性，轴线与转心的不确定度界分别不超过 3° 和 3 mm。 +候选只按训练图像的平方误差选择；留出数据不用于选优，所选候选验证失败不能改选另一候选。 +候选家族还须在准备段留出图像上区分：按运动角度分组，比较与拟合一致的平方误差损失， +差值为 `alternative_rms² - (candidate_rms + 0.03 px)²`，保留原像素容差。 +同时通过单侧均值和符号秩检验，再对完整比较集采用 Holm 逐步校正，名义族错误水平保持 0.01。 +`image_model_selection.py` 统一负责候选选择、同帧角色对齐、损失比较和多重校正； +诊断记录原始/校正后概率、选择策略和实际源 URDF 约束，失败时也保留这些证据。 +缺失、未收敛或无法评估的替代家族不能被当作已排除；几何等价的家族才可合并。 +这些检查是图像几何的条件性证据，不是独立毫米精度证明，不能替代第四轮 JSON／URDF 验收。 + +确认成功后冻结轴线、参考旋转、转心、安装关系及其来源图像。 +`image_motion_projection.py` 后续每帧只反解当前图像角度，不重新拟合这些几何参数。 +保留 1.5 px 重投影、75° 倾角及单帧角度不确定度检查；信息不足或角度仍有歧义时丢弃该帧， +按现有方向覆盖和最多一次同速重扫处理。35° 连续性和失联后的确认仍由公共 tracker 检查。 +Tag 中心接近转轴本身不再是必然失败条件，是否可解取决于全部角点提供的几何信息。 +准备段暂时缺帧或候选不确定时，每组尚未冻结零位的关节最多沿原归零路径补采一次； +已确认几何但零位图像短缺时，保持几何和姿态，只重新打开一次图像窗口。 +两个恢复方式共享一次额度，保存 `joint_zero_recovery`，不延长成无限重试。 +多机位已有部分几何冻结、保持姿态改变、设备故障和诊断采集不会触发重新拟合。 +恢复后仍无法确认时不冻结零位、不发布；已有零位和旧数据不能被重新解释后拼接。 + +`motion_branch_observation` 保存准备段图像、角点、候选及 SDK 状态；`motion_branch_initialization` 保存各假设残差、 +冻结模型、来源图像和内容哈希。新姿态来源为 `image_constrained_current_image`，保留原始 IPPE 候选， +同时记录 `frozen_image_model` 与 `image_model_sha256`。每个运动 Tag 的 `motion_evidence_id` 绑定首次证据， +`JointZeroReference.branch_references` 将它与零位、机位和分支版本关联。 +正式读取与收尾校验这些关联，缺少证据、篡改来源或删除表头均不能降级为旧图像确认； +`motion_branch_provenance.json` 只证明证据链完整，不是精度证书。 +恢复时共享来源只导入一次，关节零位及历史验证记录在该关节实际验证后导入，支持恢复期间再次中止。 +正式回放用同一冻结模型复算同帧角点，核对父链、相机、Tag 尺寸及实际提交姿态，不重新拟合模型。 +旧 `confirmed_branch_v1/v2/v3` 及 v4 三维候选约束记录仍按原语义读取，不能自动升级或恢复成新 v5 证据。 +`motion_evidence.py` 的旧三维筛选入口保留供历史兼容和诊断。这里 v5 仅指姿态策略，最终标定 JSON 仍为 schema v3。 + +真实 O6 `20260914_175815` 的旧三维筛选只读回放中，yaw 与 pitch 分别存在唯一通过几何条件的家族； +但 pitch→IP 的四种父子组合全部超差，IP 转心 RMS 约 4.9–11.4 mm，完整拇指链仍为 `unresolved`。 +这批图像来自旧零位冻结之后的单轮诊断扫描，只能用于反事实回归，不能代替新准备段证据、 +覆盖旧零位或证明 JSON＋URDF 精度。 +可复算摘要见[本次分支修复回放](../../calibration_output/O6_RIGHT_001/20260914_175815/motion_branch_fix_review/README.md)， +测试夹具同时保留“yaw/pitch 可分”和“IP 不得强行通过”两类断言。 +后续 `20260914_200044` 的图像求解回放使用其真实归零准备段冻结模型，后续 838 帧可复算通过当前图像检查； +这仍是单轮诊断回放，不是完整四轮精度验收，也不会生成正式 JSON 或 URDF。 + +`20260915_104446` 正式实采中，拇指三个关节各冻结 10 帧零位并完成四轮， +pitch／IP 的旋转及指令单表子项均通过；yaw 旋转通过，但指令单表 MAE 为 1.0454°，超过 1°。 +两场独立会话均观察到稳定方向差，该批观测即使允许每个指令任取一个输出角，MAE 下界仍超过 1°。 +这不是通过重新拟合单表能够消除的误差；方向分支的诊断通过也不能冒充单表通过。 +详见[拇指实测子项报告](../../calibration_output/O6_RIGHT_001/20260915_104446/motion_holdout_review/README.md)。 +后续 `20260915_111401` yaw 保持实测在 64／128／192 各双向保持 10 秒；末 3 秒的方向差 +分别为 2.2965°／2.7885°／2.1248°，每个窗口均有 90 帧可靠观测，同一目标的 SDK 反馈相同。 +0.5–1 秒至末段的角度变化最多约 0.038°，延长等待没有消除方向差。 +本轮仅启动顶部机位，不能据此确定具体机械原因或证明整手精度；继续保留单值 JSON 与原验收门槛。 +复现脚本、时间曲线与来源哈希见[双向保持实测](../../calibration_output/O6_RIGHT_001/20260915_111401/yaw_hold_review/README.md)。 +`20260915_104446` 会话在小指准备求解超时后停止，未完成整手空间／FK 验收、未发布 JSON／URDF。 +小指同源准备段离线求解只需约 3.42 秒，但候选仍未达到统计区分要求；运行时超时来源尚待定位， +详见[小指准备段核查](../../calibration_output/O6_RIGHT_001/20260915_104446/side_preparation_review/README.md)。 +后续会话另存 `motion_solver_diagnostic.jsonl`，记录请求来源哈希、版本、求解耗时和提交接受／拒绝， +以区分求解未返回与迟到结果被丢弃；该文件不参与测量授权，也不修改已冻结的原始记录。 +此诊断接线在上述实采之后添加，不能补证该次超时原因。 + +固定基准另有明确的静止约束：锁定后始终使用首次冻结的相机局部姿态，将其投影到当前角点验证， +不因 IPPE 两支重投影误差排名反转而更换姿态或暂停。仍执行原有 1.5 px 重投影和 75° 倾角限制， +并保留当前 IPPE 候选与原有角点漂移监测。当前图像无法验证冻结姿态时丢弃依赖该基准的观测， +记录 `fixed_reference_pose_unverified`，不将几帧图像质量不合格判为不可恢复的姿态分支冲突。 +恢复须默认至少 5 个合格帧且跨越至少 0.1 秒，始终核验同一冻结姿态，恢复完成前也禁止缓存兜底。 +尚未使用该机位的动作可以继续；当前任务持续缺少有效数据时，仍按原有零位超时、稳态采样和最多一次同速重扫处理。 +固定基准实际连续 10 帧漂移超过 5 px 仍使整场暂停，包括当前任务未使用的机位。 +此约束仅用于 Profile 声明的 `fixed_reference`， +不用于随关节运动的 Tag，也不证明初始分支物理正确或当前外参已经实测验证。 方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖， 以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。 拟合或最终文件验收失败不重新自动运动、不更新发布指针。 +`independent_rotation_holdout_failed` 表示前三轮冻结旋转模型未通过第四轮检查，发生在空间求解前。 +错误原因包含失败关节、机位和 MAE/P95/最大误差；`fit_diagnostics.json.rotation_holdout` +另存完整 SO(3)、轴向、偏轴统计及最坏图像身份。验收使用完整旋转误差，不能仅凭投影角度曲线重复判定通过。 +新采样行的 `pnp_observation_evidence` 保存父子 Tag 的当前角点、尺寸、已选姿态和候选诊断， +相机矩阵引用同会话的 `rectified_camera_model`。缓存固定基准明确标注来源，不伪造当前帧的角点或候选。 +可见且通过冻结姿态投影验证的基准标为 `verified_fixed_reference`，保存当前角点、IPPE 候选和实际投影误差； +完全未检测到时的缓存仍标为 `cached_fixed_reference`。候选回放保持同一冻结基准并复核前者的当前角点证据。 +短程诊断报告以 `fixed_reference_summary` 单独统计基准的当前图像投影误差， +不会把固定输出姿态的零跳变误当作独立测量结果。 +不合格姿态事件另外记录当时的动作阶段、指令与同步反馈，便于区分准备动作期间的异常与正式采样异常。 +角点与候选记录用于诊断；其中分支状态、版本及实际选中姿态也用于核验冻结参考的一致性， +不使历史采集自动获得缺失的观测证据。O12 的轨迹几何评分仍只使用训练轮， +但正式收尾不得用评分更好的另一支替换已冻结的训练或第四轮姿态；超过同一公共等价界时明确拒绝发布。 +历史记录仍可在离线诊断入口分析候选，不能借旧格式标记绕过正式产物的冻结分支检查。 所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、 -有效数据、基准、断点来源，以及中文原因、建议和原始诊断。耗时拟合在后台执行，不阻塞反馈与状态。 +有效数据、基准、断点来源，以及中文原因、建议和原始诊断。 +ROS 收尾拟合在独立进程执行：只读取采集结束时已同步的日志前缀，后续图像不进入拟合； +主进程继续接收反馈并检查固定基准，阶段事件按原顺序回传，最终发布仍由主进程持锁完成。 +中止时终止计算子进程，不允许子进程更新发布指针。`finalization_timing.json` 保存实际耗时。 标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件； 空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。 收到开始请求时重新检查设备状态；实时 CameraInfo 参数必须与受保护外参中的指纹一致。 @@ -155,7 +505,7 @@ linkerhand_calibration/ ├── profiles/ 加载、静态验证、Tag/CAD 观测关系 ├── core/domain/ Profile、样本、CalibrationResult、状态 ├── core/geometry/ 相机、变换、PnP 兼容导出 - │ └── tag_pose/ IPPE、连续跟踪、刚性组、轨迹分支选择 + │ └── tag_pose/ IPPE、连续跟踪、冻结前运动证据、刚性组和轨迹选择 ├── core/fitting/ 运动曲线、线性耦合、固定安装 │ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收 ├── core/urdf/ 授权修正、FK、范围与最终文件验收 @@ -169,6 +519,9 @@ linkerhand_calibration/ ├── runtime/execution.py Profile 任务与运动效果调度 ├── runtime/motion_execution.py 平滑轨迹与位移到位判定 ├── runtime/capture.py 多机位视觉/反馈采集 + ├── runtime/branch_initialization.py 最后归零准备段证据与一次性几何确认 + ├── runtime/branch_tracking.py 冻结约束选候选，原子提交同帧姿态与证据 + ├── runtime/motion_provenance.py 读取和收尾核验来源内容、零位与样本关联 ├── runtime/scan_quality.py 唯一方向数据门 ├── runtime/reference_lock.py 开始后固定基准 ├── runtime/resume.py 整场断点证据检查 @@ -193,7 +546,8 @@ linkerhand_calibration/ PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值， ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。 -公共分支容差仍为 `0.03 px`，重投影质量、倾角、过滤原因和连续跟踪恢复策略保持一致。 +公共分支容差仍为 `0.03 px`，重投影质量和倾角限制保持一致；首次确认、参考冻结与失联恢复 +使用同一公共分支策略，ROS 和离线链路均不能绕过它。 `CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测； 耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。 @@ -242,25 +596,66 @@ v1/v2 按原语义读取，不会自动升级为独立实测；保留的 v2 数 有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。 manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。 `measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。 -主 JSON 以原始 URDF 关节名为键，每个关节仅包含 `sdk_channel` 和 `angle_rad`； +主 JSON 以原始 URDF 关节名为键，单值关节包含 `sdk_channel` 和 `angle_rad`； O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。 -字节型号固定 256 项：SDK 指令 `v` 对应 `angle_rad[v]`，只能在训练指令实际覆盖 0–255 时导出。 +字节型号每条曲线固定 256 项：SDK 指令 `v` 对应所选曲线的第 `v` 项，只能在训练指令实际覆盖 0–255 时导出。 输出均是修正 URDF 的 q_output，不再加零偏。顶层记录身份、单位、完整 `baseline_command` 和共同零位约定。 独立观测的主动和被动关节直接导出各自视觉曲线，同一驱动通道可绑定多个关节。 显式复制的关节使用来源关节的同一组节点与角度，不重新拟合、归零或由 mimic 推导；接收通道保持不变。 -最终单条曲线在前三轮拟合中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。 -第四轮分别检验两个方向；超差时禁止发布，不隐藏切换分支。 -URDF mimic 单独用前三轮成对视觉角度拟合 `q_child≈a*q_parent`，共同零位下 offset=0； -复制的被动关节使用对应来源的线性近似，证据仍归属于来源关节。 -其近似误差不会否决已达标的完整映射，但结构、授权字段与机械限位始终强制检查。 +默认单值曲线在指定的稳态训练数据中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。 +独立稳态验证分别检验两个方向；超差时禁止发布，不隐藏切换分支。 -验收分别记录：最终 JSON 对第四轮独立视觉角度；完整 JSON＋URDF FK 对第四轮 Tag 位姿； -同一主动映射配合标准 mimic 的近似损失。前两项决定精度通过，第三项仅报告。 +O6 侧摆 `rh_thumb_cmc_yaw` 通过 `artifacts.directional_command_joints` 显式启用双向指令曲线。 +v3 JSON 顶层保存同名声明，所列关节额外包含 `increasing_rad` / `decreasing_rad`； +方向指原始 SDK 数值增加/减少，与关节角度正负无关。其他关节继续使用单值表。 +两条曲线分别只用所声明的稳态训练数据拟合，共用已冻结零位、各自满足 baseline=0； +独立稳态验证按实采方向读取最终序列化曲线，仍要求 MAE≤1°、P95≤2°、最大≤3°。 +报告分别保存运动与指令映射的训练／验证分区及来源图像，限位推导仍不得使用任一验证分区。 +`angle_rad` 保留为均值参考；双向关节在非 baseline 输入且方向未知时禁止用均值代替。 +读取器须从 baseline 初始化，停住时保持到达方向，允许端点换向；行程中途反转尚无小回环标定证据，会明确拒绝。 +字节指令的方向判断与查表共用 `floor(v+0.5)` 量化规则；连续小数步进不能绕过方向切换检查。 +两条主回环曲线不构成任意反转、负载或多轴工况的精度证明。旧读取器会拒绝新增关节字段，须同步更新。 +修正 URDF 保留原被动关节 ``，单独拖动父关节滑条时，末节按 +`q_child ≈ multiplier × q_parent + offset` 联动。标准 URDF 不能将查找表表达为 mimic。 +前三轮成对视觉角度用于拟合线性近似，优化变量为父关节两端对应的子关节角度； +两端都限制在子关节原有输出范围内，因此整个联动行程合法，无需放大限位或裁剪小负角。 +偏移是近似直线在 baseline 的误差，不能当作新的物理零位；报告明确保存 +`baseline_error_rad`、`physical_zero_correction: false` 和独立留出误差。 +JSON 指令/反馈曲线、几何零位不因近似拟合改变。需要实测非线性运动时使用完整 JSON， +消费程序显式设置全部关节角，并避免 URDF mimic 再次覆盖末节角度。 + +原始 CAD 限位默认仍是硬边界。Profile 可逐关节显式声明 `calibrated_motion_envelope`， +将该 CAD 限位视为本次要测定的运动范围；只有已授权的 `limit.lower/upper` 可据此修改。 +O6 右手采用此策略，范围由前三轮运动反馈映射、独立稳态训练映射的实测包络及共同零位确定， +不增加安全余量，不用第四轮扩展范围，也不为容纳 mimic 越界而放大范围。 +SDK 安全指令域不变，IP／DIP 的 CAD 零位不变。复制关节的范围证据仍属于来源关节。 +报告和 manifest 的验收数据保存 `motion_range_evidence`，包含来源限位、零偏、训练轮次、 +样本身份与新范围，并在收尾重新验证。O6 尚无独立机械安全角度界的实测证据， +该策略不代表对碰撞、过载或任意多轴姿态作出安全认证。 + +验收分别记录：最终 JSON 对独立稳态验证角度；完整 JSON＋URDF FK 对独立验证 Tag 位姿； +仅使用 URDF mimic 的近似损失。前两项决定实测精度通过，第三项如实报告，不冒充非线性精度。 +manifest 同时保存 `urdf_joint_angle_source: calibration_json`、`urdf_mimic_policy` 和两种回放结果。 门限保持角度 MAE≤1°、P95≤2°、最大≤3°，Tag 位置 P95≤3 mm。 +平行转轴的零位计算先消除轴上参考点的任意坐标，再比较相机平面中的轴间方向； +改变同一轴线的表示点不能改变标定零位。这是所有型号共用的几何规则。 独立精度结论只覆盖实际观测的关节和 Tag；复制关节另检查来源、查表一致性、通道与机械范围， 不把来源关节的第四轮误差称为接收关节精度。混合产物 manifest 使用 `measured_and_transferred_json_urdf_v3`，列出实测和复制覆盖。 -两种 FK 使用相同的训练冻结安装与基座；第四轮不拟合零位、曲线、Tag 安装或基座。 +FK 使用训练冻结的安装与基座；第四轮不拟合零位、曲线、Tag 安装或基座。 +正式图像采集还要求核验第四轮及独立稳态验证的原始 Tag 四角，沿用冻结的安装、基座 +和受哈希保护的相机参数。主观测的独立三维位姿必须解释同帧原始角点，四角重投影 RMS +不得超过原 1.5 px；最终落盘 JSON＋修正 URDF 与独立位姿的差异仍按上述角度和位置门限检查。 +最终 FK 的总像素误差单独报告，不能将包含允许运动误差的总量等同于图像测量噪声。 +额外机位没有独立三维测量时，仅检查是否存在满足原物理误差范围、且重投影误差不超过 +1.5 px 的局部刚性位姿。这个数值见证不是真实位姿测量，不写回安装、零位、曲线或相机； +各运动任务分别检查，不能用其他任务中的静止图像稀释误差。它不能替代必要的独立三维验收。 +缺少独立测量或原始图像、相机身份不一致、测量像素或物理误差超限均不能发布。 +失败时保留 `final_image_diagnostics.json`；通过后在 manifest 记录 `final_file_image_holdout` +及规则版本 `independent_measurement_and_physical_consistency_v2`，分别列出独立精度与额外视角相容性。 +训练阶段若 Tag 与整条运动链不满足刚性一致性，`tag_installation_diagnostics.json` +一次列出全部 Tag 的角度／位置 P95、门限和通过状态，不只显示遇到的第一个失败 Tag。 +该失败不能直接归因于硬件松动，也不能作为自动重做整场采集的依据。 `calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、 实测关节的零位原始参考帧、完整指令/反馈/到达方向、来源和训练/holdout 指标。 @@ -280,8 +675,11 @@ ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \ 需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。 桥检查三文件哈希、拒绝越域输入， -v3 全部关节角由 JSON 显式计算，完整 FK 必须启用 `independent_mimic_angles=True`，不加第二次零偏。 -标准 `robot_state_publisher` 的 mimic 更新会覆盖被动角；仅检查其加载兼容性，不用它验证完整非线性运动。 +v3 全部关节角由 JSON 显式计算，不加第二次零偏。完整 FK 使用 +`independent_mimic_angles=True`，以 JSON 给出的末节角度覆盖线性近似。 +标准 `robot_state_publisher` 和普通 URDF 网页查看器会执行 mimic，适合检查近似联动； +仅把完整 JointState 发给标准 RSP，不能让它停止覆盖被动角。 +消费实测非线性曲线的仿真程序必须明确采用 JSON 角度，关闭重复的线性被动更新。 仿真输出不能与实机反馈话题同名。 MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。 外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。 @@ -301,6 +699,41 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \ 旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。 当前坐标规则、生产职责与验收范围见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。 -当前代码审查、已修复问题与未完成的精度验收见 -[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间； -USB/驱动积压情况下，图像与反馈的时间戳接近不能证明曝光时刻同步，不能据此宣称实机精度通过。 +早期需求审查见 [2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)； +当前修改依据、实机结果与未完成的精度验收见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。 +相机和反馈目前均采用下述测量时间；时间配对正确仍须经过独立空间验收。 + + +### 实时调度边界 + +公共 ROS 节点顺序处理控制和反馈消息；耗时图像估计由每视角有界工作队列执行。 +图像最多在原同步容差内等候真实反馈时间覆盖，队列不反向持锁查询会话。 +增加执行器线程数不能提高共享状态的处理能力；反馈超时仍按真实测量判断。 +暂停或验收失败的会话仅提供诊断/候选文件，不能当作已发布的标定结果。 +调度对照依据与验证范围见 `CALIBRATION_PIPELINE_REVIEW.md` 第七轮评估。 + + +### 相机与反馈时间 + +连续扫描将 CAN 实测反馈与相机曝光中点配对。MVS 取帧/图像发布时间不参与替代曝光时间； +设备时钟由独立锁存换算到 ROS 时钟，换算依据保存在会话的 `camera_timing_.jsonl`。 +四型号共用 `unified_engine_v8_all_view_images`，保留设备曝光时间策略；旧采集只能按其原策略诊断，不能混用。 +报告中的 `feedback_u8` 表示来自字节通道的测量值：插值到曝光时间后可以是小数， +拟合、内存映射和报告读取均按声明的分段线性曲线计算，不再次取整。 +`command_u8` 仍按实际发送的整数字节查表；旧版整数表保留原读取规则。 +相机时钟修正不会将被动关节的非线性曲线转成 URDF mimic；仿真仍须读取 JSON 查找表。 + +### 同一运动中的所有机位原始角点 + +四轮扫描、必要稳态点和关节基准采样期间，所有机位均保存 `image_observation_frame`。 +记录包含曝光时间、完整指令/反馈及方向、轮次与尝试编号、相机矩阵和可见 Tag 的二维角点。 +它经过原检测质量与采集窗口检查，但不依赖 PnP 分支或任务的主机位。 +缺少同步反馈时保留明确的空值，不能将该帧当成有效角度测量。 + +例如 O6 顶部 ID7 与正面 ID2 属于同一末节：弯曲和侧摆时两机位的可见角点都应保存， +以便检查整条运动链。其他机位的记录不会增加停点或运动次数，也不会替代主机位的采样覆盖要求。 +同机位重复帧不重复入库；暂停、切换运动和结束采集时仍按同一会话身份撤销迟到数据。 +最终 JSON/URDF 的图像验收使用通过采集检查的第四轮和独立稳态验证窗口内全部机位角点。 +主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接 +投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 SDK 条件不一致时拒绝发布。 +这不表示联合求解或外参自校准已经通过验证。 diff --git a/src/linkerhand_calibration/config/g20_right_product.yaml b/src/linkerhand_calibration/config/g20_right_product.yaml index 097cee9..ecc2df5 100644 --- a/src/linkerhand_calibration/config/g20_right_product.yaml +++ b/src/linkerhand_calibration/config/g20_right_product.yaml @@ -1,7 +1,7 @@ schema_version: 3 profile_id: G20/right/g20_right_19/v1 profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml -profile_config_sha256: f73c8a391a8feb6cb3028fbd8278d3b18d6e2e875980235f7797fd8de2efbac1 +profile_config_sha256: 958f74c476730636d2dadb58a30296a7cdf0b25ad00345252754f9c1b6b50c7f model: G20 side: right tag_layout: g20_right_19 diff --git a/src/linkerhand_calibration/config/l6_right_product.yaml b/src/linkerhand_calibration/config/l6_right_product.yaml index 21d3b53..9b2a5ea 100644 --- a/src/linkerhand_calibration/config/l6_right_product.yaml +++ b/src/linkerhand_calibration/config/l6_right_product.yaml @@ -1,7 +1,7 @@ schema_version: 3 profile_id: L6/right/l6_right_8/v1 profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml -profile_config_sha256: 733094679e5477bbe97a633f4fb1ce25242804b667cd435ef9132f491e6ef247 +profile_config_sha256: d241e87ded3a29d416a876b27f1a251f8daf2f98e3676ab277264f1bdfaf5c60 model: L6 side: right tag_layout: l6_right_8 diff --git a/src/linkerhand_calibration/config/o12_right_product.yaml b/src/linkerhand_calibration/config/o12_right_product.yaml index 67c1161..c6aa662 100644 --- a/src/linkerhand_calibration/config/o12_right_product.yaml +++ b/src/linkerhand_calibration/config/o12_right_product.yaml @@ -1,7 +1,7 @@ schema_version: 3 profile_id: O12/right/o12_right_16/v1 profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml -profile_config_sha256: d0c1e2ac62f96162612f0ec45c25c6f467d045c1c1030b5be34ccc115dca4cf9 +profile_config_sha256: 652c2a82dfc7532b4f62c42cebdad57b169435cf950c5f2ee910cc67c7972621 model: O12 side: right tag_layout: o12_right_16 @@ -15,7 +15,7 @@ sdk: package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170 transport: hcan setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash - config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml + config: package://linkerhand_calibration/config/o12_sdk.yaml config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c cameras: diff --git a/src/linkerhand_calibration/config/o12_sdk.yaml b/src/linkerhand_calibration/config/o12_sdk.yaml new file mode 100644 index 0000000..706fe46 --- /dev/null +++ b/src/linkerhand_calibration/config/o12_sdk.yaml @@ -0,0 +1,45 @@ +# OmniHand Pro 2025 (O12) Configuration +# connection_type: "zlgcan" | "hcan" | "socketcan" | "rs485" | "usb" +# request_interval_ms: min gap between requests, SDK default 0 = no throttling, range 0..100 +# frame_recv_timeout_ms: single-frame reply wait, SDK default 50, range 10..1000 + +/**: + ros__parameters: + # This workstation uses one O12 right hand through HCAN device 0/channel 0. + # Leave left_hand undefined so the node does not try to open a second hand. + right_hand: + hand_device_id: 1 + connection_type: "hcan" + canfd_device_id: 0 + canfd_channel_id: 0 + # O12 POSITION streams set-points and has no G20-style firmware speed + # interpolation, so do not throttle the 50 Hz calibration command stream. + request_interval_ms: 0 + frame_recv_timeout_ms: 100 + show_data_details: false + + # Alternative ZLG CANFD example: + # right_hand: + # hand_device_id: 1 + # connection_type: "zlgcan" + # canfd_device_id: 0 + # canfd_channel_id: 0 + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: false + # socketcan example + # left_hand: + # hand_device_id: 1 + # connection_type: "socketcan" + # can_interface: "can0" + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: true + + # right_hand: + # hand_device_id: 1 + # connection_type: "socketcan" + # can_interface: "can1" + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: true diff --git a/src/linkerhand_calibration/config/o6_right_8_tags.yaml b/src/linkerhand_calibration/config/o6_right_8_tags.yaml index 5d06f3e..2c21dc9 100644 --- a/src/linkerhand_calibration/config/o6_right_8_tags.yaml +++ b/src/linkerhand_calibration/config/o6_right_8_tags.yaml @@ -3,7 +3,7 @@ image_transport: raw qos_profile: sensor_data family: 36h11 - size: 0.016 + size: 0.0165 max_hamming: 0 detector: threads: 4 @@ -16,14 +16,14 @@ tag: ids: [0, 1, 2] frames: [front_base, thumb_pitch, thumb_ip] - sizes: [0.016, 0.016, 0.016] + sizes: [0.0165, 0.0165, 0.0165] /o6_calibration/side/apriltag/apriltag: ros__parameters: image_transport: raw qos_profile: sensor_data family: 36h11 - size: 0.016 + size: 0.0165 max_hamming: 0 detector: threads: 4 @@ -36,14 +36,14 @@ tag: ids: [3, 4, 5] frames: [side_base, pinky_pitch, pinky_dip] - sizes: [0.016, 0.016, 0.016] + sizes: [0.0165, 0.0165, 0.0165] /o6_calibration/top/apriltag/apriltag: ros__parameters: image_transport: raw qos_profile: sensor_data family: 36h11 - size: 0.016 + size: 0.0165 max_hamming: 0 detector: threads: 4 @@ -56,4 +56,4 @@ tag: ids: [6, 7] frames: [top_base, thumb_yaw] - sizes: [0.016, 0.016] + sizes: [0.0165, 0.0165] diff --git a/src/linkerhand_calibration/config/o6_right_product.yaml b/src/linkerhand_calibration/config/o6_right_product.yaml index ef7c46b..78b48e0 100644 --- a/src/linkerhand_calibration/config/o6_right_product.yaml +++ b/src/linkerhand_calibration/config/o6_right_product.yaml @@ -1,7 +1,7 @@ schema_version: 3 profile_id: O6/right/o6_right_8/v1 profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml -profile_config_sha256: df6c4abb3c9d48b186c1bd6ab304ea9c0bbab54f9381ff222d5d8d3a3f65babe +profile_config_sha256: 72bf56b13eb2a0d4d14a5be42838b2ffc176722f470fe1a3ae5fd84604e1df4d model: O6 side: right tag_layout: o6_right_8 @@ -14,25 +14,25 @@ cameras: front: serial_number: DB2163742 camera_name: hikrobot_front_DB2163742 - camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml side: serial_number: DB2163749 camera_name: hikrobot_side_DB2163749 - camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml top: serial_number: DB2163739 camera_name: hikrobot_top_DB2163739 - camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml + camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml artifacts: source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62 - camera_extrinsics: config/o6_three_camera_extrinsics.yaml - camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4 + camera_extrinsics: config/o6_three_camera_extrinsics_20260915_192517.yaml + camera_extrinsics_sha256: d057183eaba592149ec4c63c4ceab8a70e1a0c9d7cf2460a1c8b7e86a6ca6638 calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml - calibration_config_sha256: 15adaec462d0895dc68d19760e8f333c62e57786d124f22cfc481007a3d976a4 + calibration_config_sha256: 49ce8a0d317695994c5b306e3a3a28d0470b8769c77f1173ca935178430587db tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml - tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e + tag_config_sha256: 219b8aa906fc9e2a13f3bbede46f542ff176ef2c7f60190f0dd2508993f04593 release: required_independent_passes: 1 diff --git a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml index e92130d..229fe2d 100644 --- a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml @@ -21,9 +21,9 @@ o6_calibration: torque_u8: 80 repetitions: 4 preflight_checkpoints_u8: [255, 127, 0] - tag_size_m: 0.016 + tag_size_m: 0.0165 tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7] - tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + tag_size_overrides_m: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165] minimum_detection_rate: 0.95 minimum_joint_frame_rate: 0.85 diff --git a/src/linkerhand_calibration/config/profiles/g20_right_19.yaml b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml index aad9158..cbb3155 100644 --- a/src/linkerhand_calibration/config/profiles/g20_right_19.yaml +++ b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml @@ -1503,7 +1503,9 @@ artifacts: - urdf_zero_publication publish_corrected_urdf: true acquisition: - policy_version: unified_engine_v5_joint_zero + command_capture_mode: separate + joint_zero_timeout_seconds: 5.0 + policy_version: unified_engine_v8_all_view_images mapping_probe_maximum_rad: 0.0 automatic_rescan_limit: 1 minimum_valid_samples: 40 diff --git a/src/linkerhand_calibration/config/profiles/l6_right_8.yaml b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml index 8acb0e3..50535b7 100644 --- a/src/linkerhand_calibration/config/profiles/l6_right_8.yaml +++ b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml @@ -347,7 +347,9 @@ artifacts: - l6_partial_v1 publish_corrected_urdf: true acquisition: - policy_version: unified_engine_v5_joint_zero + command_capture_mode: separate + joint_zero_timeout_seconds: 5.0 + policy_version: unified_engine_v8_all_view_images mapping_probe_maximum_rad: 0.0 automatic_rescan_limit: 1 minimum_valid_samples: 40 diff --git a/src/linkerhand_calibration/config/profiles/o12_right_16.yaml b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml index 15a2b94..0de20db 100644 --- a/src/linkerhand_calibration/config/profiles/o12_right_16.yaml +++ b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml @@ -744,6 +744,9 @@ measurement: pose_axis_line_required: false zero: cad_zero_assumptions: + thumb_mcp: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured. + pinky_pip: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured. + ring_pip: User accepts recipient CAD zero at SDK baseline; motion is copied from pinky_pip, not independently measured. index_dip: User accepts original CAD joint zero as the physical baseline; not independently measured. middle_dip: User accepts original CAD joint zero as the physical baseline; not independently @@ -844,7 +847,6 @@ zero: - pinky_mcp_pitch depth_free_axis_projection: true accept_validated_zero_in_confidence_interval: true - project_axis_gauge_before_image: true axis_order: - thumb_cmc_roll - thumb_cmc_yaw @@ -929,7 +931,9 @@ artifacts: - full_sdk_range_v2 publish_corrected_urdf: true acquisition: - policy_version: unified_engine_v5_joint_zero + command_capture_mode: separate + joint_zero_timeout_seconds: 5.0 + policy_version: unified_engine_v8_all_view_images mapping_probe_maximum_rad: 0.05235987755982989 urdf: authorized_fields: @@ -997,8 +1001,10 @@ urdf: - mimic.multiplier - mimic.offset ring_pip: + - mimic.multiplier - mimic.offset ring_dip: + - mimic.multiplier - mimic.offset joint_coverage: thumb_cmc_roll: measured_static_dynamic diff --git a/src/linkerhand_calibration/config/profiles/o6_right_8.yaml b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml index e85eeed..c5f949c 100644 --- a/src/linkerhand_calibration/config/profiles/o6_right_8.yaml +++ b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml @@ -55,31 +55,40 @@ vision: - role: front_base fixed_reference: true id: 0 + size_m: 0.0165 - role: thumb_pitch fixed_reference: false id: 1 + size_m: 0.0165 - role: thumb_ip fixed_reference: false id: 2 + size_m: 0.0165 - name: side tags: - role: side_base fixed_reference: true id: 3 + size_m: 0.0165 - role: pinky_pitch fixed_reference: false id: 4 + size_m: 0.0165 - role: pinky_dip fixed_reference: false id: 5 + size_m: 0.0165 - name: top tags: - role: top_base fixed_reference: true id: 6 + size_m: 0.0165 - role: thumb_yaw fixed_reference: false id: 7 + size_m: 0.0165 + link: rh_thumb_distal common_frame: calibration_common extrinsic_reference_view: front extrinsics_quality_limits: @@ -329,6 +338,8 @@ scope: default_scope: partial artifacts: output_schema_version: 3 + directional_command_joints: + - rh_thumb_cmc_yaw calibration_filename: o6_right_{serial_number}_partial_calibration.json corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf protected_input_fields: @@ -343,7 +354,12 @@ artifacts: - o6_partial_v1 publish_corrected_urdf: true acquisition: - policy_version: unified_engine_v5_joint_zero + joint_zero_timeout_seconds: 5.0 + command_capture_mode: separate + steady_training_nodes: 9 + steady_extra_training_nodes: + pinky_pitch_dip_side: [239.0] + policy_version: unified_engine_v8_all_view_images mapping_probe_maximum_rad: 0.0 automatic_rescan_limit: 1 minimum_valid_samples: 40 @@ -358,6 +374,40 @@ acquisition: fixed_reference_maximum_drift_px: 5.0 fixed_reference_confirmation_frames: 10 urdf: + limit_policies: + rh_pinky_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_thumb_cmc_pitch: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_middle_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_index_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_ring_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_thumb_cmc_yaw: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_pinky_dip: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_thumb_ip: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_middle_dip: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_ring_dip: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. + rh_index_dip: + source_limits: calibrated_motion_envelope + evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes. authorized_fields: rh_pinky_mcp_pitch: - limit.lower @@ -386,18 +436,28 @@ urdf: rh_ring_dip: - mimic.multiplier - mimic.offset + - limit.lower + - limit.upper rh_index_dip: - mimic.multiplier - mimic.offset + - limit.lower + - limit.upper rh_pinky_dip: - mimic.multiplier - mimic.offset + - limit.lower + - limit.upper rh_thumb_ip: - mimic.multiplier - mimic.offset + - limit.lower + - limit.upper rh_middle_dip: - mimic.multiplier - mimic.offset + - limit.lower + - limit.upper joint_coverage: rh_pinky_mcp_pitch: measured_static_dynamic rh_thumb_cmc_pitch: measured_static_dynamic diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 7dab455..039ebe7 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -29,6 +29,8 @@ def _launch_stack(context): ) from linkerhand_calibration.profiles import load_hand_profile from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics + from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture + from linkerhand_calibration.runtime.observation_scope import required_observation_views model = LaunchConfiguration("model").perform(context).strip().upper() hand_type = LaunchConfiguration("hand_type").perform(context).lower() @@ -48,7 +50,11 @@ def _launch_stack(context): raise ValueError("launch identity differs from the protected Profile") except ValueError as error: raise RuntimeError(str(error)) from error - views = contract.typed_profile.vision.view_names + diagnostic = resolve_diagnostic_capture( + contract.typed_profile, + LaunchConfiguration("diagnostic_capture").perform(context).strip(), + ) + views = required_observation_views(contract.typed_profile, diagnostic) requested_tag_config = LaunchConfiguration("tag_config").perform(context) if not requested_tag_config: raise RuntimeError("tag_config is required; use calibrate_hand --config") @@ -110,9 +116,9 @@ def _launch_stack(context): for view in views } if any(not serial for serial in camera_serials.values()): - raise RuntimeError("all Profile camera serial numbers are required") + raise RuntimeError("all required observation camera serial numbers are required") if len(set(camera_serials.values())) != len(views): - raise RuntimeError("Profile camera serial numbers must be unique") + raise RuntimeError("required observation camera serial numbers must be unique") cameras = [] components = [] @@ -153,6 +159,8 @@ def _launch_stack(context): ), "image_width": 1624, "image_height": 1240, + "timestamp_journal_path": str(Path(LaunchConfiguration("session_dir").perform(context)) + / f"camera_timing_{view}.jsonl"), "frame_rate": ParameterValue( LaunchConfiguration("camera_frame_rate"), value_type=float, @@ -194,15 +202,10 @@ def _launch_stack(context): plugin="AprilTagNode", name="apriltag", namespace=detector_namespace, - parameters=[ - str(tag_config), - { - "detector.decimate": ParameterValue( - LaunchConfiguration("apriltag_decimate"), - value_type=float, - ) - }, - ], + # Detector settings belong to the protected model YAML. + # A generic launch default must not silently replace its + # full-resolution setting for the small calibration Tags. + parameters=[str(tag_config)], remappings=[ ("image_rect", rect_topic), ("camera_info", info_topic), @@ -318,6 +321,9 @@ def _launch_stack(context): "commands_enabled": ParameterValue( LaunchConfiguration("commands_enabled"), value_type=bool ), + "diagnostic_capture": ParameterValue( + LaunchConfiguration("diagnostic_capture"), value_type=str + ), }, ], ) @@ -364,6 +370,10 @@ def _launch_stack(context): ), *cameras, vision, + Node(package="linkerhand_calibration", executable="tag_border_filter_node", + name="tag_border_filter", output="screen", + arguments=["--profile-config", profile_path, "--profile-sha256", expected, + "--views", *views]), sdk, calibration, bag, @@ -400,7 +410,6 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), DeclareLaunchArgument("gain_db", default_value="0.0"), DeclareLaunchArgument("auto_exposure", default_value="false"), - DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), DeclareLaunchArgument("can_interface", default_value="can0"), DeclareLaunchArgument("calibration_speed", default_value="15"), DeclareLaunchArgument("camera_extrinsics_file", default_value=""), @@ -430,6 +439,7 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument("profile_config", default_value=""), DeclareLaunchArgument("commands_enabled", default_value="true"), + DeclareLaunchArgument("diagnostic_capture", default_value=""), DeclareLaunchArgument("start_cameras", default_value="true"), DeclareLaunchArgument("start_sdk", default_value="true"), DeclareLaunchArgument("record_bag", default_value="false"), diff --git a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py index 188a9bc..79e09fb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py +++ b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py @@ -109,6 +109,7 @@ class Observation: class StateSample: stamp_ns: int position_u8: tuple[float, ...] + channel_stamps_ns: tuple[int, ...] = () def interpolate_state_u8( @@ -127,6 +128,29 @@ def interpolate_state_u8( raise ValueError("maximum_skew_ns must be non-negative") if not samples: return None + if any(sample.channel_stamps_ns for sample in samples): + # A multi-frame SDK does not observe all motors at the same instant. + # De-duplicate each channel by its CAN receipt, then interpolate it + # independently. Repeated publication never creates extra support. + samples = tuple(sample for sample in samples if sample.stamp_ns >= stamp_ns-maximum_skew_ns + and sample.channel_stamps_ns and min(sample.channel_stamps_ns) <= stamp_ns+maximum_skew_ns) + if not samples: + return None + count = len(samples[0].position_u8) + channels = [dict() for _ in range(count)] + for sample in samples: + if len(sample.position_u8) != count or len(sample.channel_stamps_ns) != count: + return None + for index, (stamp, value) in enumerate(zip(sample.channel_stamps_ns, sample.position_u8)): + if stamp in channels[index] and channels[index][stamp] != value: + return None + channels[index][stamp] = value + matches = [interpolate_state_u8(tuple(StateSample(stamp, (value,)) + for stamp, value in sorted(channel.items())), stamp_ns, maximum_skew_ns=maximum_skew_ns) + for channel in channels] + if any(match is None for match in matches): + return None + return tuple(match[0][0] for match in matches), max(match[1] for match in matches) stamps = [int(sample.stamp_ns) for sample in samples] index = bisect_left(stamps, int(stamp_ns)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py b/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py new file mode 100644 index 0000000..9a73ad2 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py @@ -0,0 +1,165 @@ +"""Map camera frame clocks to host time before pairing images with feedback. + +The MVS frame counter timestamps exposure start. USB delivery/publication is +later. Clock latches bracket the device/host correspondence independently of +hand motion, images, or fitted calibration curves. +""" + +from dataclasses import asdict, dataclass +import json +import math +from pathlib import Path +import time + + +CAMERA_TIMING_POLICY = "mvs_latched_exposure_time_v1" + + +@dataclass(frozen=True) +class ClockLatch: + device_tick: int + before_ns: int + after_ns: int + + @property + def midpoint_ns(self): + return (self.before_ns+self.after_ns)//2 + + @property + def half_width_ns(self): + return (self.after_ns-self.before_ns)/2 + + +class DeviceFrameClock: + """A nominal device clock with short, measured host-time brackets. + + A new latch is checked against the previous one before it can change the + mapping. This rejects wrong counter units, resets and host clock jumps. + Periodic updates bound accumulated drift; none depend on image delivery. + """ + + def __init__(self, ticks_per_second): + if not math.isfinite(ticks_per_second) or ticks_per_second <= 0: + raise ValueError("invalid camera clock frequency") + self.ticks_per_second = ticks_per_second + self.anchor = None + self.last_frame_tick = None + self.last_stamp_ns = None + + def elapsed_ns(self, ticks): + return round(ticks*1_000_000_000/self.ticks_per_second) + + def update(self, latches): + if not latches or any(x.device_tick <= 0 or x.before_ns <= 0 + or not 0 <= x.after_ns-x.before_ns <= 2_000_000 for x in latches): + raise ValueError("camera clock latch is invalid or too uncertain") + previous = self.anchor or latches[0] + if self.anchor is None and latches[-1].before_ns-latches[0].after_ns < 50_000_000: + raise ValueError("camera clock units need an independent elapsed-time check") + for current in latches: + elapsed = self.elapsed_ns(current.device_tick-previous.device_tick) + host_elapsed = current.midpoint_ns-previous.midpoint_ns + tolerance = current.half_width_ns+previous.half_width_ns+1_000_000 + if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance: + raise ValueError("camera/host clock discontinuity or counter unit mismatch") + self.anchor = min(latches, key=lambda x: x.half_width_ns) + + def timestamp(self, tick, *, received_ns, exposure_us): + if (self.anchor is None or not 0 <= received_ns-self.anchor.midpoint_ns <= 2_000_000_000 + or not math.isfinite(exposure_us) or exposure_us < 0): + raise ValueError("camera frame has no fresh clock or valid exposure") + if self.last_frame_tick is not None and tick <= self.last_frame_tick: + raise ValueError("camera frame counter reset or duplicate frame") + stamp = self.anchor.midpoint_ns+self.elapsed_ns(tick-self.anchor.device_tick)+round(exposure_us*500) + if stamp > received_ns+2_000_000 or (self.last_stamp_ns is not None and stamp <= self.last_stamp_ns): + raise ValueError("camera image time is future or out of order") + self.last_frame_tick, self.last_stamp_ns = tick, stamp + return stamp + + +class MvsCameraTiming: + """The camera-specific clock latch and its optional append-only journal.""" + + def __init__(self, camera, mvs, clock_ns, *, exposure_us, journal_path=""): + self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns + self.exposure_us = exposure_us + self.frame_clock = None + self.next_refresh = 0. + self.journal = None + if journal_path: + path = Path(journal_path) + path.parent.mkdir(parents=True, exist_ok=True) + self.journal = path.open("a", encoding="utf-8") + + def _integer(self, name): + value = self.mvs.MVCC_INTVALUE_EX() + status = self.camera.MV_CC_GetIntValueEx(name, value) + if status != 0: + raise ValueError(f"camera clock cannot read {name}:0x{status:08x}") + return int(value.nCurValue) + + def _latch(self): + before = self.clock_ns() + status = self.camera.MV_CC_SetCommandValue("DeviceTimestampLatch") + if status != 0: + raise ValueError(f"camera clock latch failed:0x{status:08x}") + tick = self._integer("DeviceTimestamp") + return ClockLatch(tick, before, self.clock_ns()) + + def _write(self, row): + if self.journal is not None: + self.journal.write(json.dumps(row, separators=(",", ":"))+"\n") + + def start(self): + # The installed USB camera firmware returns ticks/second for this + # feature (despite its XML unit label). Verify that interpretation + # against elapsed host time; do not assume it for another firmware. + self.frame_clock = DeviceFrameClock(self._integer("DeviceTimestampIncrement")) + latches = [] + for index in range(40): + if index: + time.sleep(.02) + latches.append(self._latch()) + valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000] + if len(valid) >= 8 and valid[-1].before_ns-valid[0].after_ns >= 50_000_000: + self._update(latches) + return + raise ValueError("camera clock has no bounded-latency startup latches") + + def _update(self, latches): + valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000] + if not valid: + # A delayed USB control transaction provides no new clock evidence. + # Keep the original two-second freshness bound while trying again. + self._write(dict(kind="camera_clock_latch_rejected", latches=[asdict(x) for x in latches])) + self.next_refresh = time.monotonic()+.02 + return + self.frame_clock.update(valid) + self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY, + ticks_per_second=self.frame_clock.ticks_per_second, + latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor))) + if self.journal is not None: + self.journal.flush() + self.next_refresh = time.monotonic()+.5 + + def refresh_if_due(self): + if time.monotonic() >= self.next_refresh: + self._update([self._latch() for _ in range(3)]) + + def timestamp(self, info, received_ns): + tick = (int(info.nDevTimeStampHigh)<<32)|int(info.nDevTimeStampLow) + # Auto-exposure preview uses the exact exposure-start timestamp when + # the SDK supplies no per-frame duration. Calibration fixes exposure. + measured_exposure = float(info.fExposureTime) + exposure = measured_exposure if measured_exposure > 0 else self.exposure_us + stamp = self.frame_clock.timestamp(tick, received_ns=received_ns, exposure_us=exposure) + self._write(dict(kind="camera_frame_time", frame_number=int(info.nFrameNum), + device_tick=tick, sdk_host_stamp_ms=int(info.nHostTimeStamp), + received_ns=received_ns, stamp_ns=stamp, exposure_us=exposure, + timestamp_reference="exposure_midpoint" if exposure else "exposure_start")) + return stamp + + def close(self): + if self.journal is not None: + self.journal.close() + self.journal = None diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py index 13131b9..9eca14f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py @@ -39,6 +39,8 @@ def build_typed_profile(): right, key=KEY, namespace="/o6_left_transferred", + # This historical mirror has no measured left-hand range evidence. + urdf_limit_policies={}, command=replace( right.command, command_index_by_joint=command_index, @@ -116,6 +118,7 @@ def build_typed_profile(): artifacts=replace( right.artifacts, output_schema_version=2, + directional_command_joints=frozenset(), calibration_filename=( "o6_left_{serial_number}_transferred_calibration.json" ), diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py index 6cd02ad..31fb5a6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py @@ -7,6 +7,7 @@ from .domain import ( CalibrationProfile, CalibrationResult, JointMapping, + JointLimitPolicy, CalibrationStatus, CommandLayout, MeasurementPolicy, @@ -53,6 +54,7 @@ __all__ = [ "CalibrationProfile", "CalibrationResult", "JointMapping", + "JointLimitPolicy", "CalibrationStatus", "CommandLayout", "DIRECTION_DECREASING", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py index 13f9a36..879af70 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py @@ -31,9 +31,14 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None: def append_jsonl_many( - path: str | Path, payloads: Iterable[Mapping[str, Any]] + path: str | Path, payloads: Iterable[Mapping[str, Any]], *, durable: bool = True, ) -> None: - """Durably append a batch while paying the fsync cost only once.""" + """Append one ordered batch, durable by default. + + Continuous observations may defer fsync until a reference/checkpoint, + pause, or explicit sync_jsonl barrier. They are still flushed before + returning; only complete, durable scan units can authorize resume. + """ destination = Path(path) destination.parent.mkdir(parents=True, exist_ok=True) lines = [ @@ -45,6 +50,13 @@ def append_jsonl_many( with destination.open("a", encoding="utf-8") as stream: stream.write("\n".join(lines) + "\n") stream.flush() + if durable: + os.fsync(stream.fileno()) + + +def sync_jsonl(path: str | Path) -> None: + """Persist all previously flushed records before hashing or publishing.""" + with Path(path).open("rb") as stream: os.fsync(stream.fileno()) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py index 8eb504a..3af428e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py @@ -19,6 +19,7 @@ from .profile import ( ZeroSolvePolicy, JointZeroSpec, KnownBaselineGeometry, + JointLimitPolicy, validate_profile, ) from .result import CalibrationResult, JointMapping @@ -42,6 +43,7 @@ __all__ = [ "JointZeroSample", "JointZeroSpec", "KnownBaselineGeometry", + "JointLimitPolicy", "CalibrationStatus", "CommandLayout", "MeasurementPolicy", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 035432d..119d9a3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -8,7 +8,7 @@ from pathlib import PurePath from typing import Mapping -ACQUISITION_POLICY_VERSION = "unified_engine_v5_joint_zero" +ACQUISITION_POLICY_VERSION = "unified_engine_v8_all_view_images" @dataclass(frozen=True, order=True) @@ -337,9 +337,21 @@ class AcquisitionPolicy: require_endpoint_observations: bool = False endpoint_tolerance_01: float = 2.0 / 255.0 steady_training_nodes: int = 9 + command_capture_mode: str = "interleaved" + steady_extra_training_nodes: Mapping[str, tuple[float, ...]] = field(default_factory=dict) steady_minimum_samples: int = 3 steady_window_seconds: float = 0.2 steady_timeout_seconds: float = 2.0 + joint_zero_timeout_seconds: float = 2.0 + + +def observation_streams(profile, task): + """Primary and declared secondary views of each measured joint.""" + for name in task.joints: + yield "joint", name, profile.measurement.measurements[name] + secondary = profile.measurement.cross_view_sources.get(name) + if secondary is not None: + yield "observation_joint", name, profile.measurement.measurements[secondary] @dataclass(frozen=True) @@ -364,6 +376,22 @@ class ArtifactPolicy: publication_pointer: str = "latest_passed" session_compatibility_tokens: frozenset[str] = frozenset() publish_corrected_urdf: bool = False + directional_command_joints: frozenset[str] = frozenset() + + +@dataclass(frozen=True) +class JointLimitPolicy: + """An explicit interpretation of CAD limits, separate from SDK safety. + + Independent safety bounds, when known, are in original CAD coordinates. + Their absence never means that a calibrated motion envelope is a hardware + safety certificate. + """ + + source_limits: str + evidence_source: str + independent_safety_limits_cad_rad: tuple[float, float] | None = None + safety_evidence_source: str = "" @dataclass(frozen=True) @@ -388,6 +416,7 @@ class CalibrationProfile: # transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal, # and mimic_nominal. joint_coverage: Mapping[str, str] = field(default_factory=dict) + urdf_limit_policies: Mapping[str, JointLimitPolicy] = field(default_factory=dict) @property def curve_input_domain(self) -> str: @@ -723,6 +752,16 @@ def validate_profile(profile: CalibrationProfile) -> None: artifacts = profile.artifacts if artifacts.output_schema_version < 1: errors.append("artifact schema version must be positive") + if artifacts.directional_command_joints and (artifacts.output_schema_version != 3 + or not artifacts.directional_command_joints <= profile.measurement.measurements.keys()): + errors.append("directional command curves require measured v3 joints") + for target, source in profile.measurement.transferred_motion_sources.items(): + if (target in artifacts.directional_command_joints) != (source in artifacts.directional_command_joints): + errors.append("transferred command curves must retain their source direction policy") + for task in profile.motion.tasks: + if artifacts.directional_command_joints.intersection(task.joints): + if profile.command.baseline_values[task.command_index] not in (task.start_value, task.end_value): + errors.append("directional command mapping requires an endpoint baseline") for label, filename in ( ("calibration", artifacts.calibration_filename), ("corrected URDF", artifacts.corrected_urdf_filename), @@ -742,9 +781,33 @@ def validate_profile(profile: CalibrationProfile) -> None: valid_urdf_fields ): errors.append("URDF authorization contains an unsupported field") + for name, policy in profile.urdf_limit_policies.items(): + if name not in all_joints or not isinstance(policy, JointLimitPolicy): + errors.append(f"URDF limit policy requires a known movable joint:{name}") + continue + if policy.source_limits != "calibrated_motion_envelope" or not policy.evidence_source.strip(): + errors.append(f"URDF motion envelope requires explicit source-limit interpretation:{name}") + if not {"limit.lower", "limit.upper"} <= set(profile.urdf_authorized_fields.get(name, ())): + errors.append(f"URDF motion envelope lacks limit authorization:{name}") + bounds = policy.independent_safety_limits_cad_rad + if bounds is None: + if policy.safety_evidence_source: + errors.append(f"URDF safety evidence has no independent bounds:{name}") + elif (len(bounds) != 2 or not all(math.isfinite(v) for v in bounds) + or bounds[0] >= bounds[1] or not policy.safety_evidence_source.strip()): + errors.append(f"URDF independent safety bounds require finite limits and evidence:{name}") acquisition = profile.acquisition + if acquisition.command_capture_mode not in {"interleaved", "separate"}: + errors.append("unsupported command capture mode") + task_by_key = {task.key: task for task in profile.motion.tasks} + for key, values in acquisition.steady_extra_training_nodes.items(): + task = task_by_key.get(key) + if task is None or any(not math.isfinite(value) or not min(task.start_value, task.end_value) < value < max(task.start_value, task.end_value) for value in values): + errors.append(f"extra steady nodes must lie inside a declared task:{key}") if acquisition.steady_training_nodes < 3 or acquisition.steady_minimum_samples < 3: errors.append("steady mapping requires at least three nodes and three images per node") + if not acquisition.steady_timeout_seconds <= acquisition.joint_zero_timeout_seconds <= 10.0: + errors.append("joint zero capture timeout must be between the steady timeout and 10 seconds") if not 0 < acquisition.steady_window_seconds < acquisition.steady_timeout_seconds <= 2.0: errors.append("steady acquisition must have a finite stability window and <=2 second wait") if acquisition.policy_version != ACQUISITION_POLICY_VERSION: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py index 3b111b6..0e36fb4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py @@ -2,8 +2,9 @@ from __future__ import annotations -from dataclasses import asdict, dataclass +from dataclasses import asdict, dataclass, replace import math +import re from typing import Mapping, Sequence import numpy as np @@ -46,6 +47,22 @@ class JointZeroSample: JointZeroPose.from_mapping(row["parent_pose_common"]), JointZeroPose.from_mapping(row["child_pose_common"])) +@dataclass(frozen=True) +class JointZeroBranchReference: + """A moving physical Tag's immutable pre-zero motion authorization.""" + + view: str + tag_role: str + branch_revision: int + motion_evidence_id: str + + def __post_init__(self): + if (not self.view or not self.tag_role or type(self.branch_revision) is not int + or self.branch_revision < 1 or not isinstance(self.motion_evidence_id, str) + or re.fullmatch(r"[0-9a-f]{64}", self.motion_evidence_id) is None): + raise ValueError("joint_zero_motion_evidence_invalid") + + @dataclass(frozen=True) class JointZeroReference: joint: str @@ -55,6 +72,7 @@ class JointZeroReference: session_epoch: int motion_version: int samples: tuple[JointZeroSample, ...] + branch_references: tuple[JointZeroBranchReference, ...] = () def quaternion(self, view): rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in self.samples if s.view == view]) @@ -81,7 +99,57 @@ class JointZeroReference: **{key: tuple(s[key]) for key in ("relative_quaternion_xyzw", "relative_translation_xyz_m", "command", "feedback", "arrival_directions")}}) for s in row["samples"]) return cls(str(row["joint"]), str(row["task_key"]), float(row["baseline_command"]), - int(row["channel"]), int(row["session_epoch"]), int(row["motion_version"]), samples) + int(row["channel"]), int(row["session_epoch"]), int(row["motion_version"]), samples, + tuple(JointZeroBranchReference(**item) for item in row.get("branch_references", ()))) + + +def joint_zero_moving_roles(profile, joint): + """Physical roles that require motion evidence for this joint's zero.""" + names = (joint, profile.measurement.cross_view_sources.get(joint)) + roles = set() + for name in names: + if name is None: + continue + measurement = profile.measurement.measurements[name] + for view in profile.vision.views: + if view.name == measurement.view: + roles.update((view.name, tag.role) for tag in view.tags + if tag.role in (measurement.parent_role, measurement.child_role) and not tag.fixed_reference) + return roles + + +def _branch_references(profile, joint, rows): + allowed = joint_zero_moving_roles(profile, joint) + result, observed = {}, {} + for row in rows: + for item in row.get("pnp_observation_evidence", {}).values(): + key = (row["view"], item.get("tag_role")) + diagnostics = item.get("candidate_diagnostics", {}) + verified, identity = diagnostics.get("motion_evidence_verified"), diagnostics.get("motion_evidence_id") + observed.setdefault(key, []).append((verified, identity)) + if not identity and verified is not True: + continue # Legacy image confirmation remains readable as legacy. + if (key not in allowed or verified is not True or item.get("pose_source") not in { + "current_image", "image_constrained_current_image"} + or diagnostics.get("observation_stamp_ns") != row["image_stamp_ns"] + or diagnostics.get("branch_status") != "tracking"): + raise ValueError(f"joint_zero_motion_evidence_unconfirmed:{joint}") + reference = JointZeroBranchReference(*key, diagnostics.get("branch_revision"), identity) + if key in result and result[key] != reference: + raise ValueError(f"joint_zero_motion_evidence_changed:{joint}") + result[key] = reference + for key, reference in result.items(): + required = sum(row["view"] == key[0] for row in rows) + if len(observed[key]) != required or any( + verified is not True or identity != reference.motion_evidence_id for verified, identity in observed[key]): + raise ValueError(f"joint_zero_motion_evidence_incomplete:{joint}") + return tuple(result[key] for key in sorted(result)) + + +def _validate_branch_bindings(profile, reference): + keys = [(item.view, item.tag_role) for item in reference.branch_references] + if len(set(keys)) != len(keys) or not set(keys) <= joint_zero_moving_roles(profile, reference.joint): + raise ValueError(f"joint_zero_motion_evidence_binding_invalid:{reference.joint}") def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, session_epoch, motion_version): @@ -91,9 +159,19 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi secondary = profile.measurement.cross_view_sources.get(joint) if secondary: views.add(profile.measurement.measurements[secondary].view) - samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows) if session_epoch < 1 or motion_version < 1: raise ValueError(f"joint_zero_session_version_invalid:{joint}") + # Incomplete windows are checked by the control timer repeatedly. Check + # their image coverage before constructing and validating every pose. + minimum = profile.acquisition.fixed_reference_minimum_frames + for view in views: + count = len({int(row["image_stamp_ns"]) for row in rows if row["view"] == view}) + if count < minimum: + measurements = [profile.measurement.measurements[n] for n in (joint, secondary) if n is not None] + roles = {role for m in measurements if m.view == view for role in (m.parent_role, m.child_role)} + ids = [t.tag_id for v in profile.vision.views if v.name == view for t in v.tags if t.role in roles] + raise ValueError(f"joint_zero_samples_missing:{joint}:{view}:required_tags={ids}:{count}/{minimum}") + samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows) for sample in samples: if sample.image_stamp_ns < 1 or sample.sample_id != f"{sample.view}:{sample.image_stamp_ns}": raise ValueError(f"joint_zero_image_identity_invalid:{joint}") @@ -105,14 +183,8 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi or not np.allclose(rp.inv().apply(np.asarray(child.translation_xyz_m)-parent.translation_xyz_m), sample.relative_translation_xyz_m, atol=1e-8, rtol=0)): raise ValueError(f"joint_zero_relative_pose_inconsistent:{joint}") - minimum = profile.acquisition.fixed_reference_minimum_frames for view in views: selected = [s for s in samples if s.view == view] - if len({s.sample_id for s in selected}) < minimum: - measurements = [profile.measurement.measurements[n] for n in (joint, secondary) if n is not None] - roles = {role for m in measurements if m.view == view for role in (m.parent_role, m.child_role)} - ids = [t.tag_id for v in profile.vision.views if v.name == view for t in v.tags if t.role in roles] - raise ValueError(f"joint_zero_samples_missing:{joint}:{view}:required_tags={ids}:{len(selected)}/{minimum}") rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in selected]) if np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1): raise ValueError(f"joint_zero_not_stable:{joint}:{view}") @@ -132,7 +204,7 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi if sample.arrival_directions[task.command_index] != expected: raise ValueError(f"joint_zero_arrival_changed:{joint}") return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[task.command_index], - task.command_index, session_epoch, motion_version, samples) + task.command_index, session_epoch, motion_version, samples, _branch_references(profile, joint, rows)) def read_joint_zero_references(profile, records): @@ -164,6 +236,8 @@ def read_joint_zero_references(profile, records): for s in reference.samples] checked = build_joint_zero_reference(profile, name, rows, session_epoch=reference.session_epoch, motion_version=reference.motion_version) + _validate_branch_bindings(profile, reference) + checked = replace(checked, branch_references=reference.branch_references) if checked.channel != reference.channel: raise ValueError(f"joint_zero_channel_changed:{name}") result[name] = checked diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py index 55c2956..e478c1a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py @@ -82,7 +82,9 @@ class JointMapping: if not math.isfinite(value) or not self.knots[0] <= value <= self.knots[-1]: raise ValueError(f"SDK input outside fitted support:{self.joint}:{value}") branch = getattr(self, direction + "_rad") if direction else self.angle_rad - if self.input_domain.endswith("_u8"): + # Commands become actual wire bytes. Feedback can be interpolated + # between CAN receipts at an image's exposure time and stays continuous. + if self.input_domain == "command_u8": value = math.floor(value + 0.5) return float(np.interp(value, self.knots, branch)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py index a936ff1..a4be453 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py @@ -3,9 +3,18 @@ import numpy as np +def command_training_cycles(profile): + return (4,) if profile.acquisition.command_capture_mode == "separate" else (0, 1, 2) + + +def command_holdout_cycle(profile): + return 5 if profile.acquisition.command_capture_mode == "separate" else 3 + + def command_nodes(profile, task, *, holdout=False): lo, hi = sorted((task.start_value, task.end_value)) train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes) + train = np.unique(np.r_[train, profile.acquisition.steady_extra_training_nodes.get(task.key, ())]) if profile.artifacts.output_schema_version >= 3: baseline = profile.command.baseline_values[task.command_index] if not lo <= baseline <= hi: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py index 2b5cb66..49526bf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py @@ -13,6 +13,7 @@ class TaskStatus: cycle: int | None = None cycle_count: int = 4 direction: str = "" + capture_stage: str = "motion" required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field( default_factory=dict ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py index c982f3b..18e553b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py @@ -12,11 +12,13 @@ from ..domain.result import JointMapping from ..urdf.acceptance import angular_metrics from .curve import isotonic_nonincreasing from .spatial import measure_joint_curve_observation -from ..domain.sampling import command_nodes +from ..domain.sampling import command_nodes, command_training_cycles, command_holdout_cycle def fit_command_mappings(profile, fit, records_by_joint): mappings, metrics, applicability = {}, {}, {} + training_cycles = command_training_cycles(profile) + holdout_cycle = command_holdout_cycle(profile) for joint in sorted(fit.output_mappings): donor = joint if profile.artifacts.output_schema_version >= 3 else profile.zero.transferred_zero_sources.get(joint, joint) rows = records_by_joint.get(donor, ()) @@ -36,9 +38,11 @@ def fit_command_mappings(profile, fit, records_by_joint): channel = fit.output_mappings[joint].motor_index source_channel = fit.output_mappings[donor].motor_index branches = {} + directional = joint in profile.artifacts.directional_command_joints task = next(t for t in profile.motion.tasks if donor in t.joints) lo, hi = sorted((task.start_value, task.end_value)) - knots = command_nodes(profile, task) + from .command_sampling import declared_training_nodes + knots = declared_training_nodes(profile, task, rows) sign = profile.command.joint_directions[source_channel] if profile.artifacts.output_schema_version >= 3: sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1 @@ -46,7 +50,7 @@ def fit_command_mappings(profile, fit, records_by_joint): values = [] for knot in knots: medians = [] - for cycle in (0, 1, 2): + for cycle in training_cycles: selected = [r for r in rows if r["cycle"] == cycle and r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)] if len(selected) < profile.acquisition.steady_minimum_samples: @@ -61,6 +65,11 @@ def fit_command_mappings(profile, fit, records_by_joint): if np.max(np.abs(projected-values)) > math.radians(1): raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}") branches[direction] = tuple(float(v) for v in projected) + if directional: + from .anchored_mapping import anchored_mean + branches = {direction: anchored_mean(knots, values, values, + fit.joint_zero_references[joint].baseline_command, sign) + for direction, values in branches.items()} mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"])) if profile.artifacts.output_schema_version >= 3: from .anchored_mapping import anchored_mean @@ -68,7 +77,7 @@ def fit_command_mappings(profile, fit, records_by_joint): fit.joint_zero_references[joint].baseline_command, sign) mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots, mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None) - validation = [r for r in rows if r["cycle"] == 3] + validation = [r for r in rows if r["cycle"] == holdout_cycle] expected = command_nodes(profile, task, holdout=True) for direction in ("increasing", "decreasing"): for knot in set(expected): @@ -76,7 +85,7 @@ def fit_command_mappings(profile, fit, records_by_joint): for r in validation) < profile.acquisition.steady_minimum_samples: raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}") errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]), - "" if profile.artifacts.output_schema_version >= 3 else r["direction"]) + r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "") - observed[r["sample_id"]] for r in validation] quality = angular_metrics(errors) if not quality.passed: @@ -84,14 +93,15 @@ def fit_command_mappings(profile, fit, records_by_joint): mappings[joint] = mapping from dataclasses import asdict metrics[joint] = {**asdict(quality), "independently_measured": donor == joint, - "source_joint": donor, "training_cycles": [0, 1, 2], "holdout_cycle": 3} + "source_joint": donor, "training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle, + "training_sample_ids": sorted(r["sample_id"] for r in rows if r["cycle"] in training_cycles)} if profile.artifacts.output_schema_version >= 3: - directional = {direction: angular_metrics([error for error, row in zip(errors, validation) + branch_metrics = {direction: angular_metrics([error for error, row in zip(errors, validation) if row["direction"] == direction]) for direction in ("increasing", "decreasing")} - if not all(value.passed for value in directional.values()): - raise ValueError(f"single_command_curve_direction_holdout_failed:{joint}") - metrics[joint]["by_direction"] = {key: asdict(value) for key, value in directional.items()} - training = [r for r in rows if r["cycle"] in {0, 1, 2}] + if not all(value.passed for value in branch_metrics.values()): + raise ValueError(f"command_curve_direction_holdout_failed:{joint}") + metrics[joint]["by_direction"] = {key: asdict(value) for key, value in branch_metrics.items()} + training = [r for r in rows if r["cycle"] in training_cycles] vectors = np.asarray([r[f"command_vector_{profile.command.unit}"] for r in training]) held = {str(i): float(np.median(vectors[:, i])) for i in range(profile.command.command_count) if i != source_channel} @@ -101,5 +111,8 @@ def fit_command_mappings(profile, fit, records_by_joint): applicability[joint] = {"task": task.key, "held_command_values": held, "scope": "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False, "branch_initialization": "full_range_endpoint_reset"} + if directional: + applicability[joint].update(command_mapping="directional", direction_source="raw_sdk_input", + interior_reversals_validated=False) return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics, command_applicability=applicability) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py new file mode 100644 index 0000000..3b8fb49 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py @@ -0,0 +1,141 @@ +"""Training-only reduction of static command knots with immutable evidence. + +The continuous curve chooses where to stop, never supplies a static command +angle. Every retained static knot is measured afresh in both directions. The +original independent validation grid and all publication gates remain intact. +""" + +import hashlib +import json +import math + +import numpy as np + +from ..domain.sampling import command_nodes +from ..domain.profile import observation_streams +from .rotation_curve import RotationObservation, fit_rotation_curve + + +SAMPLING_POLICY = "training_shape_static_knots_v1" +MAXIMUM_REMOVAL_ERROR_RAD = math.radians(.25) + + +def digest(value): + return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() + + +def training_sources(profile, task, records): + streams = {(field, name, spec.view) for field, name, spec in observation_streams(profile, task)} + latest = {} + for row in records: + if row.get("task_name") == task.key and row.get("cycle") in {0, 1, 2} and row.get("direction"): + key = (row["cycle"], row["direction"]) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + result = [] + for row in records: + if (row.get("task_name") != task.key or row.get("cycle") not in {0, 1, 2} + or row.get("sample_phase", "sweep") != "sweep" or "relative_quaternion_xyzw" not in row): + continue + if int(row.get("attempt", 1)) != latest.get((row["cycle"], row["direction"])): + continue + for field, name, view in sorted(streams): + if row.get(field) == name and row.get("view") == view: + result.append({"field": field, "joint": name, "view": view, "cycle": row["cycle"], + "direction": row["direction"], "image_stamp_ns": row["image_stamp_ns"], + "input": float(row[f"feedback_{profile.command.unit}"]), + "quaternion": list(row["relative_quaternion_xyzw"])}) + return sorted(result, key=lambda row: (row["field"], row["joint"], row["view"], row["image_stamp_ns"])) + + +def reduced_knots(grid, curves, mandatory): + """Shortest path through the original grid; each omitted knot is bounded.""" + grid, values = np.asarray(grid), np.asarray(curves) + paths = {0: (0,)} + for end in range(1, len(grid)): + candidates = [] + for start, path in paths.items(): + if any(start < index < end for index in mandatory): + continue + fraction = (grid[start:end+1]-grid[start])/(grid[end]-grid[start]) + prediction = values[:, start, None] + (values[:, end]-values[:, start])[:, None]*fraction + if np.max(np.abs(prediction-values[:, start:end+1])) <= MAXIMUM_REMOVAL_ERROR_RAD: + candidates.append((*path, end)) + paths[end] = min(candidates, key=lambda path: (len(path), path)) + return tuple(float(grid[index]) for index in paths[len(grid)-1]) + + +def build_sampling_plan(profile, task, records): + sources = training_sources(profile, task, records) + grid = command_nodes(profile, task) + knots, reason = grid, "full_grid_insufficient_training_shape" + curves = [] + try: + for field, name, spec in observation_streams(profile, task): + rows = [row for row in sources if (row["field"], row["joint"], row["view"]) == (field, name, spec.view)] + observations = [RotationObservation(f"{row['view']}:{row['image_stamp_ns']}", row["input"], + tuple(row["quaternion"]), row["cycle"], row["direction"]) for row in rows] + fitted = fit_rotation_curve(observations, input_domain=f"feedback_{profile.command.unit}", + sdk_to_joint_direction=profile.command.joint_directions[task.command_index]) + if fitted.maximum_monotonic_correction_rad > MAXIMUM_REMOVAL_ERROR_RAD: + raise ValueError("training_shape_noisy") + for branch in (fitted.increasing_rad, fitted.decreasing_rad): + curves.append(np.interp(grid, fitted.knots, branch)) + mandatory_values = {grid[0], grid[-1], grid[len(grid)//2], + profile.command.baseline_values[task.command_index], + *profile.acquisition.steady_extra_training_nodes.get(task.key, ())} + mandatory = {i for i, value in enumerate(grid) if value in mandatory_values} + knots = reduced_knots(grid, curves, mandatory) + reason = "training_shape_bounded" if len(knots) < len(grid) else "full_grid_required_by_training_shape" + except (ValueError, KeyError, TypeError, np.linalg.LinAlgError) as error: + reason = "full_grid:" + str(error) + plan = {"kind": "command_sampling_plan", "policy": SAMPLING_POLICY, "task_name": task.key, + "training_nodes": list(knots), "validation_nodes": list(command_nodes(profile, task, holdout=True)), + "source_sha256": digest(sources), "source_count": len(sources), "training_cycles": [0, 1, 2], + "maximum_removal_error_rad": MAXIMUM_REMOVAL_ERROR_RAD, "reason": reason} + return {**plan, "plan_sha256": digest(plan)} + + +def declared_training_nodes(profile, task, records): + """Read a plan in complete journals or self-contained individual units.""" + declarations = set() + for row in records: + if row.get("task_name") != task.key: + continue + if row.get("kind") == "command_sampling_plan": + if digest({k: v for k, v in row.items() if k != "plan_sha256"}) != row.get("plan_sha256"): + raise ValueError("command_sampling_plan_hash_changed") + declarations.add(tuple(row["training_nodes"])) + elif row.get("sample_phase") == "steady" and "steady_training_nodes" in row: + declarations.add(tuple(row["steady_training_nodes"])) + if len(declarations) > 1: + raise ValueError("command_sampling_plan_conflict") + if not declarations: + return command_nodes(profile, task) + nodes = next(iter(declarations)) + allowed = set(command_nodes(profile, task)) + if (len(nodes) < 3 or tuple(sorted(set(nodes))) != nodes or not set(nodes) <= allowed + or nodes[0] != min(allowed) or nodes[-1] != max(allowed)): + raise ValueError("command_sampling_nodes_invalid") + return nodes + + +def validate_sampling_plans(profile, records): + plans = {} + for row in records: + if row.get("kind") == "command_sampling_plan": + task = next(task for task in profile.motion.tasks if task.key == row["task_name"]) + if row != build_sampling_plan(profile, task, records): + raise ValueError("command_sampling_training_evidence_changed") + if task.key in plans and plans[task.key] != row: + raise ValueError("command_sampling_plan_conflict") + plans[task.key] = row + required = any(row.get("command_sampling_policy") == SAMPLING_POLICY for row in records) + for row in records: + if row.get("sample_phase") != "steady" or "steady_target" not in row: + continue + plan = plans.get(row["task_name"]) + if required or "sampling_plan_sha256" in row: + if (plan is None or row.get("sampling_plan_sha256") != plan["plan_sha256"] + or list(row.get("steady_training_nodes", ())) != plan["training_nodes"]): + raise ValueError("command_sampling_sample_unbound") + return plans diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py index ea1816f..0e4313d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py @@ -106,6 +106,7 @@ def fit_standard_mimic( cycles: Sequence[int], offset_rad: float | None, offset_observable: bool = False, + multiplier_bounds: tuple[float, float] | None = None, ) -> LinearMimicFit: """Fit actual synchronous observations, never a vendor-generated curve. @@ -139,8 +140,16 @@ def fit_standard_mimic( raise ValueError("mimic parameters are not independently observable") residual = rhs - design @ initial scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual))))) + bounds = (-np.inf, np.inf) + if multiplier_bounds is not None: + if (offset_rad is None or len(multiplier_bounds) != 2 + or not all(math.isfinite(v) for v in multiplier_bounds) + or multiplier_bounds[0] >= multiplier_bounds[1]): + raise ValueError("bounded mimic requires a fixed offset and a finite feasible interval") + bounds = multiplier_bounds + initial = np.clip(initial, *bounds) solution = least_squares(lambda parameters: design @ parameters - rhs, - initial, loss="soft_l1", f_scale=scale) + initial, loss="soft_l1", f_scale=scale, bounds=bounds) if not solution.success or not np.all(np.isfinite(solution.x)): raise ValueError("linear mimic fit failed") multiplier = float(solution.x[0]) @@ -149,6 +158,38 @@ def fit_standard_mimic( offset_rad is None, (0, 1, 2), tuple(float(v) for v in (multiplier*x + offset - y))) +def fit_range_bounded_mimic(source_joint, target_joint, active_rad, passive_rad, *, + cycles, parent_range, child_range): + """Best robust affine approximation within the unchanged joint limits. + + Optimize the two child angles at the parent's endpoints. Bounding those + two values guarantees that the entire affine motion stays in the child's + interval, including tiny negative baseline excursions. The intercept is + approximation error at baseline, NOT an observed physical zero correction. + """ + initial_fit = fit_standard_mimic(source_joint, target_joint, active_rad, passive_rad, + cycles=cycles, offset_rad=0.0) + for interval in (parent_range, child_range): + if (len(interval) != 2 or not all(math.isfinite(v) for v in interval) + or interval[0] >= interval[1]): + raise ValueError("invalid_mimic_motion_interval") + x, y = np.asarray(active_rad, dtype=float), np.asarray(passive_rad, dtype=float) + lower, upper = parent_range + fraction = (x - lower) / (upper - lower) + design = np.column_stack((1 - fraction, fraction)) + residual = np.asarray(initial_fit.residuals_rad) + scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual))))) + initial = np.clip(np.asarray(parent_range) * initial_fit.multiplier, *child_range) + solution = least_squares(lambda endpoints: design @ endpoints - y, initial, + loss="soft_l1", f_scale=scale, bounds=child_range) + if not solution.success or not np.all(np.isfinite(solution.x)): + raise ValueError("bounded_affine_mimic_fit_failed") + multiplier = float((solution.x[1] - solution.x[0]) / (upper - lower)) + offset = float(solution.x[0] - multiplier * lower) + return LinearMimicFit(source_joint, target_joint, multiplier, offset, False, + (0, 1, 2), tuple(float(v) for v in multiplier*x + offset - y)) + + @dataclass(frozen=True) class CouplingFit: source_joint: str diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py index b796171..5beeb85 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py @@ -1,9 +1,9 @@ -"""A training-only URDF approximation, separate from measured joint curves.""" +"""Training-only URDF approximations, separate from exact JSON lookup tables.""" -from dataclasses import dataclass +from dataclasses import dataclass, replace import math -from .coupling import LinearMimicFit, fit_standard_mimic +from .coupling import LinearMimicFit, fit_standard_mimic, fit_range_bounded_mimic from .observed_motion import paired_visual_observations @@ -14,16 +14,43 @@ class MimicApproximation: holdout_sample_ids: tuple[str, ...] holdout_errors_rad: tuple[float, ...] transferred_from_joint: str | None = None + multiplier_bounds: tuple[float, float] | None = None + parent_range_rad: tuple[float, float] | None = None + child_range_rad: tuple[float, float] | None = None def in_cad_coordinates(self, offsets): from dataclasses import replace + if self.parent_range_rad is not None: + raise ValueError("affine_approximation_intercept_is_not_a_physical_zero") fit = self.output_fit # The offset is supported by the TWO baseline geometry datums. return replace(fit, offset_rad=offsets[fit.target_joint]-fit.multiplier*offsets[fit.source_joint], offset_observed=True) -def fit_mimic_approximations(model, curves, records): +def shared_zero_multiplier_bounds(parent_range, child_range): + """Intersect endpoint constraints for q_child = a*q_parent. + + These ranges come from training support or mechanical bounds, never from + holdout observations. Neither measured curve nor joint range is modified. + """ + lower, upper = -math.inf, math.inf + if any(len(interval) != 2 or not all(math.isfinite(v) for v in interval) + or interval[0] >= interval[1] for interval in (parent_range, child_range)): + raise ValueError("invalid_mimic_motion_interval") + for endpoint in parent_range: + if endpoint == 0: + if not child_range[0] <= 0 <= child_range[1]: + raise ValueError("mimic_range_excludes_shared_zero") + continue + values = sorted(value / endpoint for value in child_range) + lower, upper = max(lower, values[0]), min(upper, values[1]) + if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper: + raise ValueError("no_feasible_shared_zero_mimic") + return lower, upper + + +def fit_mimic_approximations(model, curves, records, *, output_ranges=None, affine=False): result = {} for name, joint in model.joints.items(): parent = joint.mimic_joint @@ -36,10 +63,45 @@ def fit_mimic_approximations(model, curves, records): raise ValueError(f"paired_mimic_observations_missing:{name}:{cycle}:{direction}") training = [r for r in rows if r.cycle != 3] holdout = [r for r in rows if r.cycle == 3] - fit = fit_standard_mimic(parent, name, [r.source_rad for r in training], - [r.target_rad for r in training], cycles=[r.cycle for r in training], offset_rad=0.0) + bounds = None + arguments = dict(source_joint=parent, target_joint=name, + active_rad=[r.source_rad for r in training], passive_rad=[r.target_rad for r in training], + cycles=[r.cycle for r in training]) + if affine: + if output_ranges is None: + raise ValueError("affine_mimic_requires_training_motion_ranges") + fit = fit_range_bounded_mimic(**arguments, + parent_range=output_ranges[parent], child_range=output_ranges[name]) + else: + bounds = None if output_ranges is None else shared_zero_multiplier_bounds( + output_ranges[parent], output_ranges[name]) + fit = fit_standard_mimic(**arguments, offset_rad=0.0, multiplier_bounds=bounds) if not math.isfinite(fit.multiplier) or fit.multiplier == 0: raise ValueError(f"invalid_mimic_approximation:{name}") result[name] = MimicApproximation(fit, tuple(r.sample_id for r in training), - tuple(r.sample_id for r in holdout), tuple(fit.multiplier*r.source_rad-r.target_rad for r in holdout)) + tuple(r.sample_id for r in holdout), + tuple(fit.multiplier*r.source_rad + fit.offset_rad - r.target_rad for r in holdout), + multiplier_bounds=bounds, + parent_range_rad=tuple(output_ranges[parent]) if affine else None, + child_range_rad=tuple(output_ranges[name]) if affine else None) return result + + +def fit_bounded_mimic_approximations(profile, model, fit, records): + """Fit the URDF affine fallback after training establishes both ranges. + + Only independently observed pairs are fitted; explicit donor expansion is + performed afterwards. JSON continues to use the independent nonlinear maps. + """ + ranges = {} + for name, mapping in fit.output_mappings.items(): + joint = model.joints[name] + if name in profile.urdf_limit_policies or joint.mimic_joint is None: + command = fit.command_mappings[name] + ranges[name] = (min(0, mapping.bounds_rad[0], command.bounds_rad[0]), + max(0, mapping.bounds_rad[1], command.bounds_rad[1])) + else: + delta = fit.zero_offsets_rad[name] + ranges[name] = (joint.lower - delta, joint.upper - delta) + return replace(fit, mimic_approximations=fit_mimic_approximations( + model, fit.curves, records, output_ranges=ranges, affine=True)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py index 32f4c00..61f7f3f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py @@ -89,7 +89,7 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re observed[name], holdout[name] = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain, input_to_joint_direction=joint_input_direction(profile, source_model, name), - reference_xyzw=reference.quaternion(view)) + reference_xyzw=reference.quaternion(view), joint=name, view=view) references[name] = reference.input_reference(view, feedback=True) coordinates = observed elif profile.command.unit == "u8": @@ -105,7 +105,8 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re observed, coordinates, holdout, evidence, references = {}, {}, {}, {}, {} for name, rows in sorted(records_by_joint.items()): curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain, - input_to_joint_direction=joint_input_direction(profile, source_model, name)) + input_to_joint_direction=joint_input_direction(profile, source_model, name), + joint=name, view=profile.measurement.measurements[name].view) observed[name], holdout[name] = curve, errors coordinates[name], references[name] = native_coordinate_curve(curve, profile.command.baseline_values[channel_for_joint(profile, name)]) @@ -124,7 +125,9 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain, input_to_joint_direction=joint_input_direction(profile, source_model, name), reference_xyzw=(zero_references[name].quaternion(rows[0]["view"]) - if profile.artifacts.output_schema_version >= 3 else None)) + if profile.artifacts.output_schema_version >= 3 else None), + joint=name, view=profile.measurement.measurements[ + profile.measurement.cross_view_sources[name]].view) if "input_knots" not in observed[name].circle: raise ValueError("cross-view comparison requires native input knots") comparison = compare_native_views(observed[name], curve) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_transfer.py index 600e64d..4566e99 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_transfer.py @@ -42,7 +42,9 @@ def expand_profile_transfers(profile: CalibrationProfile, fit: CalibrationResult if profile.zero.transferred_zero_sources.get(target) == source and source in offsets: offsets[target] = offsets[source] methods[target] = "transferred_static_zero_on_own_cad" - if target in profile.zero.passive_joints: + # Historical fitted approximations remain transferable for replay. + # Current measured fitting emits no mimic fit; each recipient keeps its CAD coupling. + if target in profile.zero.passive_joints and approximations: if source not in fit.mimic_approximations: raise ValueError(f"motion_transfer_requires_paired_donor_mimic:{target}:{source}") donor = fit.mimic_approximations[source] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py index f2957a5..b10aa98 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py @@ -10,6 +10,7 @@ from .image_curve import fit_joint_image_curve from ..urdf.acceptance import angular_metrics from ..domain.measurement import JointCurveFit from .rotation_curve import RotationObservation, fit_rotation_curve +from .rotation_diagnostics import RotationHoldoutFailure, rotation_holdout_diagnostic def image_identity(row: Mapping[str, Any]) -> str: @@ -44,7 +45,8 @@ def paired_visual_observations(source_rows, target_rows, source_fit, target_fit) return output -def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction, reference_xyzw=None): +def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction, + reference_xyzw=None, joint=None, view=None): """Native-coordinate 3+1 hinge primitive with the shared geometric result. The metadata carries real knots, not a synthetic byte command domain. @@ -60,10 +62,12 @@ def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction fitted = fit_rotation_curve([row for row in observations if row.cycle != 3], input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction, reference_xyzw=reference_xyzw) - errors = fitted.holdout_errors([row for row in observations if row.cycle == 3]) + holdout = [row for row in observations if row.cycle == 3] + errors = fitted.holdout_errors(holdout) metrics = angular_metrics(errors) if not metrics.passed: - raise ValueError("independent_rotation_holdout_failed") + raise RotationHoldoutFailure(rotation_holdout_diagnostic( + fitted, holdout, errors, metrics, joint=joint, view=view)) hysteresis = float(np.max(np.abs(np.asarray(fitted.increasing_rad) - fitted.decreasing_rad))) curve = JointCurveFit(fitted.angle_rad, fitted.decreasing_rad, fitted.increasing_rad, {"space": "relative_rotation_3d", "input_domain": input_domain, "input_knots": fitted.knots, diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_diagnostics.py new file mode 100644 index 0000000..3279b31 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_diagnostics.py @@ -0,0 +1,78 @@ +"""Explain a failed frozen rotation holdout without changing its acceptance.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Sequence + +from ..urdf.acceptance import ErrorMetrics, angular_metrics +from .rotation_curve import RotationCurve, RotationObservation + + +@dataclass(frozen=True) +class RotationHoldoutSample: + sample_id: str + cycle: int + direction: str + input_value: float + full_so3_error_deg: float + axial_error_deg: float + off_axis_error_deg: float + + +@dataclass(frozen=True) +class RotationHoldoutDiagnostic: + joint: str | None + view: str | None + input_domain: str + full_so3: ErrorMetrics + axial: ErrorMetrics + off_axis: ErrorMetrics + worst_sample: RotationHoldoutSample + training_cycles: tuple[int, ...] = (0, 1, 2) + holdout_cycle: int = 3 + + +class RotationHoldoutFailure(ValueError): + """A rejected rotation fit with serializable, read-only sample evidence.""" + + def __init__(self, diagnostics: RotationHoldoutDiagnostic): + self.diagnostics = diagnostics + metrics = diagnostics.full_so3 + super().__init__( + "independent_rotation_holdout_failed:" + f"joint={diagnostics.joint or 'unknown'}:view={diagnostics.view or 'unknown'}:" + f"mae_deg={metrics.mae_deg:.6f}:p95_deg={metrics.p95_deg:.6f}:" + f"max_deg={metrics.maximum_deg:.6f}") + + +def rotation_holdout_diagnostic( + fitted: RotationCurve, + observations: Sequence[RotationObservation], + errors_rad: Sequence[float], + metrics: ErrorMetrics, + *, + joint: str | None, + view: str | None, +) -> RotationHoldoutDiagnostic: + """Describe the existing failed fit; never fit, re-zero or filter samples. + + Axial error compares projected log angles. Off-axis error measures the + observed rotation's departure from the frozen hinge axis. These explain + different effects; their magnitudes are not additive SO(3) components. + Only the original full SO(3) metrics determine acceptance. + """ + measurements = [fitted.measure(row.quaternion_xyzw) for row in observations] + axial = [angle - fitted.predict(row.input_value, row.direction) + for row, (angle, _) in zip(observations, measurements)] + off_axis = [error for _, error in measurements] + worst_index = max(range(len(errors_rad)), key=errors_rad.__getitem__) + worst = observations[worst_index] + return RotationHoldoutDiagnostic( + joint, view, fitted.input_domain, metrics, + angular_metrics(axial), angular_metrics(off_axis), + RotationHoldoutSample( + worst.sample_id, worst.cycle, worst.direction, worst.input_value, + math.degrees(errors_rad[worst_index]), math.degrees(axial[worst_index]), + math.degrees(off_axis[worst_index]))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py index cf21cbc..f1d0eeb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py @@ -32,7 +32,7 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi "directed_base_axis_joints", "depth_free_axis_projection", "axis_order", "same_view_axis_pair_by_offset", "parallel_root_pattern", - "accept_validated_zero_in_confidence_interval", "project_axis_gauge_before_image"} + "accept_validated_zero_in_confidence_interval"} if not data or set(data) - allowed: raise ValueError("spatial observation graph is missing or has unsupported fields") direct = tuple(profile.zero.direct_zero_joints) @@ -42,6 +42,12 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi axes = tuple(data.get("axis_order", profile.zero.axis_joints)) if set(axes) != set(profile.zero.axis_joints) or len(set(axes)) != len(axes): raise ValueError("spatial axis order differs from declared observations") + # Coordinate descent must solve an upstream zero before the downstream + # phase that depends on it. The declaration of required zeros is a set of + # requirements, not an optimization order (O6 used pitch before yaw). + if not set(direct) <= set(axes): + raise ValueError("spatial zero has no position in the declared axis order") + direct = tuple(name for name in axes if name in direct) return ZeroCalibrationProfile( hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints), stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias), @@ -60,7 +66,6 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi orientation_anchor_joint=data.get("orientation_anchor_joint"), directed_base_axis_joints=frozenset(data.get("directed_base_axis_joints", ())), accept_validated_zero_in_confidence_interval=bool(data.get("accept_validated_zero_in_confidence_interval", False)), - project_axis_gauge_before_image=bool(data.get("project_axis_gauge_before_image", False)), parallel_root_pattern=bool(data.get("parallel_root_pattern", False)), independent_observed_motion=profile.artifacts.output_schema_version >= 3) @@ -136,7 +141,6 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco offsets = {name: float(zero.direct_offsets_rad[name]) for name in profile.zero.direct_zero_joints} methods = {name: "urdf_serial_axis_geometry" for name in offsets} if measured: - from .mimic_approximation import fit_mimic_approximations for name, datum in profile.zero.known_baseline_geometry.items(): offsets[name], methods[name] = datum.cad_angle_rad, "independent_known_baseline_geometry" for name in profile.zero.cad_zero_assumptions: @@ -145,8 +149,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco tuple(curve.circle["input_knots"]), curve.angle_rad, curve.increasing_rad, curve.decreasing_rad) for name, curve in curves.items()} return CalibrationResult(curves, mappings, offsets, methods, {}, motion.holdout_errors_rad, zero, - motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references), - mimic_approximations=fit_mimic_approximations(model, curves, records_by_joint)) + motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references)) for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys(): if name not in profile.zero.cad_frozen_joints: raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py index f10ec47..42f8d64 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py @@ -93,7 +93,6 @@ class ObservationGeometry: observed_child_point: np.ndarray, view_normal_common: Sequence[float] | None = None, ) -> float: - profile = self.profile parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) if float(parent_axis @ observed_axis) < 0.0: @@ -129,19 +128,11 @@ class ObservationGeometry: # mounts therefore choose different along-axis gauges. # Reusing delta here would reintroduce that arbitrary # coordinate and turn it into a phase at oblique views. - predicted_image_input = ( - predicted_radial if profile.project_axis_gauge_before_image - else predicted_delta + predicted_radial = predicted_radial - view_normal * float( + predicted_radial @ view_normal ) - observed_image_input = ( - observed_radial if profile.project_axis_gauge_before_image - else observed_delta - ) - predicted_radial = predicted_image_input - view_normal * float( - predicted_image_input @ view_normal - ) - observed_radial = observed_image_input - view_normal * float( - observed_image_input @ view_normal + observed_radial = observed_radial - view_normal * float( + observed_radial @ view_normal ) angle_axis = view_normal if float(angle_axis @ parent_axis) < 0.0: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py index 25e3207..b2ae27c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py @@ -58,11 +58,6 @@ class ZeroCalibrationProfile: # contains it. The frozen zero still faces every geometry/cycle/holdout # check below. Default False preserves existing G20/L6/O6 decisions. accept_validated_zero_in_confidence_interval: bool = False - # Axis-line points have no unique coordinate along the axis. Remove that - # gauge before projecting a parallel-axis phase into the camera plane. - # Opt in explicitly while legacy profiles retain their reviewed policy. - project_axis_gauge_before_image: bool = False - parallel_root_pattern: bool = False common_mode_zero_joints: tuple[str, ...] = () small_offset_joints: frozenset[str] = frozenset() diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py index 2f4f3ab..9fdbfb0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py @@ -61,6 +61,17 @@ class FrozenTagInstallation: training_translation_p95_m: float +class TagInstallationFailure(ValueError): + """All role residuals from one failed rigid-chain validation.""" + + def __init__(self, diagnostics): + self.diagnostics = diagnostics + failed = [name for name, row in diagnostics.items() if not row["passed"]] + super().__init__("tag_installation_not_rigid:" + ";".join( + f"{name}:rotation_p95_deg={diagnostics[name]['rotation_p95_deg']:.6f}:" + f"translation_p95_m={diagnostics[name]['translation_p95_m']:.6f}" for name in failed)) + + def fit_tag_installations( *, source_model: UrdfKinematicModel, common_from_base, link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose], @@ -84,8 +95,8 @@ def fit_tag_installations( if previous is not None and (previous[0] != row.cycle or not np.allclose(previous[1], candidate, atol=1e-8, rtol=0)): raise ValueError(f"conflicting duplicate Tag image:{row.role}:{row.sample_id}") grouped[row.role][row.sample_id] = (row.cycle, candidate) - output = {} - for role, unique in grouped.items(): + output, diagnostics = {}, {} + for role, unique in sorted(grouped.items()): if len(unique) < minimum_samples or {value[0] for value in unique.values()} != {0, 1, 2}: raise ValueError(f"Tag installation lacks three-cycle training evidence:{role}") matrices = np.asarray([value[1] for value in unique.values()]) @@ -96,12 +107,15 @@ def fit_tag_installations( distances = np.linalg.norm(matrices[:, :3, 3] - translation, axis=1) rotation_p95 = float(np.rad2deg(np.percentile(errors, 95))) translation_p95 = float(np.percentile(distances, 95)) - if rotation_p95 > 2.0 or translation_p95 > 0.003: - raise ValueError(f"Tag installation is not rigid across training tasks:{role}:rotation_p95_deg={rotation_p95}:translation_p95_m={translation_p95}") + diagnostics[role] = {"count": len(unique), "rotation_p95_deg": rotation_p95, + "translation_p95_m": translation_p95, + "passed": rotation_p95 <= 2.0 and translation_p95 <= 0.003} transform = np.eye(4) transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform), tuple(sorted(unique)), rotation_p95, translation_p95) + if any(not row["passed"] for row in diagnostics.values()): + raise TagInstallationFailure(diagnostics) return output diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py index 2766cf8..64cd8e9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py @@ -5,11 +5,17 @@ poses, substitute SDK angles, or relax the spatial solver's acceptance gates. """ from itertools import product import math +from collections.abc import Mapping import numpy as np from scipy.spatial.transform import Rotation as R from .pnp import solve_square_tag_ippe +from .tag_pose.ippe import fixed_pose_reprojection_error +from .tag_pose.branch_confirmation import BranchConfirmation +from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS, FROZEN_BRANCH_POLICY_VERSIONS +from .tag_pose.types import SquareTagPose +from .image_motion_replay import IMAGE_POSE_SOURCE, has_image_model, replay_image_models, validate_image_sample POLICY = 'training_frozen_articulated_candidates_v1' MATRIX_SOURCE = 'CameraInfo.P[:3,:3]' @@ -41,15 +47,107 @@ def _trajectory_score(parent, child): 'score': rms/.0015 + off_axis/math.radians(1)} +def _recorded_pose(payload): + if not isinstance(payload, Mapping): + raise ValueError('frozen_branch_evidence_missing:selected_pose') + quaternion = tuple(payload.get('quaternion_xyzw', ())) + translation = tuple(payload.get('translation_xyz_m', ())) + if (len(quaternion) != 4 or len(translation) != 3 + or not np.all(np.isfinite(quaternion + translation)) + or not np.isclose(np.linalg.norm(quaternion), 1, atol=1e-5)): + raise ValueError('frozen_branch_evidence_invalid:selected_pose') + return SquareTagPose(quaternion, translation, 0.) + + +def _check_frozen_paths(rows, evidence, selected_paths, role_pairs, fixed_roles): + """A geometry preference cannot replace any already-frozen image branch. + + Revisions identify a role within its capture, not a physical installation + across sessions. JointResume verifies the latter before importing rows. + Compare each path pose with that image's original confirmed pose instead + of assuming numerically equal revisions authorize cross-session reuse. + """ + equivalence = BranchConfirmation(DEFAULT_POSE_TRACKING_PARAMETERS) + for row in rows: + stamp, view = int(row['image_stamp_ns']), row.get('view') + frame = evidence[stamp] + identity = f"joint={row['joint']}:view={view}:image={stamp}:cycle={row['cycle']}" + if (not view or frame.get('view') != view or any( + frame.get(key) != row.get(key) for key in ('cycle', 'direction', 'attempt'))): + raise ValueError(f'frozen_branch_frame_identity_changed:{identity}') + observed = row.get('pnp_observation_evidence', {}) + if not isinstance(observed, Mapping): + raise ValueError(f'frozen_branch_evidence_missing:{identity}') + for field, role in zip(('parent', 'child'), role_pairs[row['joint']]): + item = observed.get(field, {}) + if not isinstance(item, Mapping) or item.get('tag_role') != role: + raise ValueError(f'frozen_branch_evidence_missing:{identity}:role={role}') + source = item.get('pose_source') + fixed_source = source in {'cached_fixed_reference', 'verified_fixed_reference'} + if (fixed_source + != bool(frame['roles'][role].get('locked_reference'))): + raise ValueError(f'frozen_branch_pose_source_changed:{identity}:role={role}') + if fixed_source and role not in fixed_roles: + raise ValueError(f'frozen_branch_fixed_role_invalid:{identity}:role={role}') + if source == 'cached_fixed_reference' and role in fixed_roles: + revision = item.get('reference_branch_revision') + else: + diagnostics = item.get('candidate_diagnostics', {}) + if (source not in {'current_image', 'verified_fixed_reference', IMAGE_POSE_SOURCE} or not isinstance(diagnostics, Mapping) + or diagnostics.get('observation_stamp_ns') != stamp + or diagnostics.get('branch_status') != 'tracking' + or diagnostics.get('branch_frozen') is not True): + raise ValueError(f'frozen_branch_evidence_unconfirmed:{identity}:role={role}') + revision = diagnostics.get('branch_revision') + if source == 'verified_fixed_reference' and ( + diagnostics.get('fixed_reference_verified') is not True + or item.get('reference_branch_revision') != revision): + raise ValueError(f'frozen_branch_fixed_reference_unverified:{identity}:role={role}') + if isinstance(revision, bool) or not isinstance(revision, int) or revision < 1: + raise ValueError(f'frozen_branch_revision_invalid:{identity}:role={role}') + original = _recorded_pose(item.get('selected_pose')) + if source == 'verified_fixed_reference': + _check_fixed_projection(original, item, frame, role, identity) + recorded = _recorded_pose(frame['roles'][role].get('selected')) + if not equivalence.equivalent(original, recorded): + raise ValueError(f'frozen_branch_recorded_pose_changed:{identity}:role={role}') + proposed = _recorded_pose(selected_paths[stamp][role]) + if not equivalence.equivalent(original, proposed): + angle, translation = equivalence.distance(original, proposed) + raise ValueError(f'frozen_branch_reselection_forbidden:{identity}:role={role}:' + f'rotation_deg={math.degrees(angle):.6f}:translation_mm={translation*1000:.6f}') + + +def _check_fixed_projection(pose, item, frame, role, identity): + """A fixed-pose marker cannot bypass current-image geometric evidence.""" + data = frame['roles'][role] + try: + if not np.array_equal(np.asarray(item['corners_xy']), np.asarray(data['corners_xy'])): + raise ValueError('corners changed') + error = fixed_pose_reprojection_error(pose, data['corners_xy'], + tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=frame['camera_matrix']) + limit = float(data.get('maximum_reprojection_error_px', 1.5)) + if not math.isfinite(limit) or limit <= 0 or not math.isfinite(error) or error > limit: + raise ValueError('fixed pose no longer explains this image') + except (KeyError, TypeError, ValueError) as exc: + raise ValueError(f'frozen_branch_fixed_projection_invalid:{identity}:role={role}') from exc + + def resolve_chain_observations(records, *, task_key, role_pairs, - projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=()): + projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=(), + require_frozen_branches=False): """Return copied observations and auditable candidate-selection evidence. Older captures did not save P. They remain on the legacy path; K is never guessed to be P. An explicit override is for isolated offline diagnosis and must be reported as such, not advertised as a verified whole session. + A current capture automatically preserves its frozen branches; production + callers enforce this even when a journal's policy header was removed. """ rows = [dict(r) for r in records] + preserve_frozen = require_frozen_branches or any( + row.get('kind') == 'session_start' + and row.get('pose_tracking_policy_version') in FROZEN_BRANCH_POLICY_VERSIONS for row in rows) roles = tuple(dict.fromkeys(role for pair in role_pairs.values() for role in pair)) if not role_pairs or len(roles) > 8 or any(len(pair) != 2 or pair[0] == pair[1] for pair in role_pairs.values()): raise ValueError('candidate selection requires a bounded articulated observation graph') @@ -62,6 +160,8 @@ def resolve_chain_observations(records, *, task_key, role_pairs, 'projection_override_used': projection_override is not None, 'is_accuracy_certificate': False} if not evidence: + if preserve_frozen: + raise ValueError(f'frozen_branch_evidence_missing:task={task_key}:candidate_frames') return rows, {**report, 'status': 'legacy_no_corners'} if projection_override is None and any( r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()): @@ -81,6 +181,23 @@ def resolve_chain_observations(records, *, task_key, role_pairs, raise ValueError('corner replay requires training and independent holdout') if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]: raise ValueError('holdout must follow the complete training trajectory') + if any(has_image_model(item) for stamp in stamps for item in evidence[stamp]['roles'].values()): + if projection_override is not None: + raise ValueError('frozen image model cannot use a different camera projection') + lookup, verified = {}, {} + for stamp in stamps: + verified[stamp] = replay_image_models(evidence[stamp]) + if not verified[stamp]: + raise ValueError('image_motion_provenance_mixed_pose_policies') + lookup[stamp] = {role: evidence[stamp]['roles'][role]['selected'] for role in roles} + for row in chosen_rows: + stamp = int(row['image_stamp_ns']) + validate_image_sample(row, evidence[stamp], verified[stamp]) + _check_frozen_paths(chosen_rows, evidence, lookup, role_pairs, fixed_roles) + return rows, {**report, 'policy': 'frozen_image_motion_replay_v1', 'status': 'resolved', + 'frozen_branches_preserved': True, 'geometry_refitted': False, + 'training_frames': int(sum(training)), 'holdout_frames': int(sum(~training)), + 'changed_joint_rows': 0} paths = {role: [[], []] for role in roles} for stamp in stamps: frame = evidence[stamp] @@ -90,7 +207,8 @@ def resolve_chain_observations(records, *, task_key, role_pairs, if data.get('locked_reference'): if role not in fixed_roles or not data.get('selected'): raise ValueError('cached fixed reference has no frozen pose') - cs = [dict(data['selected'], reprojection_error_px=0.0)] + cs = [dict(data['selected'], + reprojection_error_px=float(data['selected'].get('reprojection_error_px', 0.0)))] else: poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix) limit = float(data.get('maximum_reprojection_error_px', 1.5)) @@ -120,6 +238,9 @@ def resolve_chain_observations(records, *, task_key, role_pairs, selected = min(scores, key=lambda s: s['score']) lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(roles, selected['branches'])} for i, stamp in enumerate(stamps)} + if preserve_frozen: + _check_frozen_paths(chosen_rows, evidence, lookup, role_pairs, fixed_roles) + report['frozen_branches_preserved'] = True changed = 0 for row in chosen_rows: stamp = int(row['image_stamp_ns']) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py new file mode 100644 index 0000000..997a747 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py @@ -0,0 +1,167 @@ +"""Read back image-constrained poses against their frozen model and raw corners. + +This never trains geometry or chooses a new trajectory. The image solver may +estimate only this image's joint angles, including on the independent holdout. +Raw IPPE candidates remain separate from the selected constrained poses. +""" + +from collections.abc import Mapping +import hashlib +import json + +import numpy as np +from scipy.spatial.transform import Rotation + +from .tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates +from .tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from .tag_pose.types import SquareTagPose + + +IMAGE_POSE_SOURCE = "image_constrained_current_image" + + +def image_model_sha256(payload): + encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) + return hashlib.sha256(encoded.encode()).hexdigest() + + +def frozen_image_model(item): + """Read a named, content-bound model; a marker alone grants no permission.""" + from .tag_pose.production_image_motion import image_motion_model_from_dict + + payload, identity = item.get("frozen_image_model"), item.get("image_model_sha256") + if not isinstance(payload, Mapping) or image_model_sha256(payload) != identity: + raise ValueError("image_motion_provenance_model_hash_invalid") + return image_motion_model_from_dict(payload) + + +def has_image_model(item): + return (item.get("pose_source") == IMAGE_POSE_SOURCE + or "frozen_image_model" in item or "image_model_sha256" in item) + + +def _pose(payload): + try: + q, t = tuple(payload["quaternion_xyzw"]), tuple(payload["translation_xyz_m"]) + error = float(payload["reprojection_error_px"]) + if (len(q) != 4 or len(t) != 3 or not np.all(np.isfinite((*q, *t, error))) + or not np.isclose(np.linalg.norm(q), 1., atol=1e-5) or error < 0): + raise ValueError("invalid pose") + return SquareTagPose(q, t, error) + except (KeyError, TypeError, ValueError) as exc: + raise ValueError("image_motion_provenance_pose_invalid") from exc + + +def _same_pose(actual, expected): + return (float((Rotation.from_quat(actual.quaternion_xyzw).inv() + * Rotation.from_quat(expected.quaternion_xyzw)).magnitude()) <= 1e-6 + and np.linalg.norm(np.subtract(actual.translation_xyz_m, expected.translation_xyz_m)) <= 1e-8 + and abs(actual.reprojection_error_px - expected.reprojection_error_px) <= 1e-6) + + +def replay_image_models(frame): + """Recompute every constrained role from the original frame, without fitting.""" + from .tag_pose.production_image_motion import select_image_motion_frame + + if (frame.get("camera_matrix_source") != "CameraInfo.P[:3,:3]" + or frame.get("input_is_rectified") is not True): + raise ValueError("image_motion_provenance_projection_unverified") + models, selected = {}, {} + for role, item in frame["roles"].items(): + if not has_image_model(item): + continue + if item.get("pose_source") != IMAGE_POSE_SOURCE: + raise ValueError("image_motion_provenance_pose_source_changed") + identity = item.get("image_model_sha256") + model = frozen_image_model(item) + models[identity] = model + if role not in {geometry.relation.child_role for geometry in model.geometry}: + raise ValueError("image_motion_provenance_role_not_in_model") + for identity, model in models.items(): + if (max(model.source_stamps_ns) >= frame["image_stamp_ns"] + or not np.array_equal(np.asarray(frame["camera_matrix"]), model.camera_matrix)): + raise ValueError("image_motion_provenance_model_not_frozen_before_image") + children = {geometry.relation.child_role for geometry in model.geometry} + roles, observations = [], [] + for role, size in model.tag_sizes: + item = frame["roles"].get(role) + if item is None or float(item.get("tag_size_m", -1)) != size: + raise ValueError("image_motion_provenance_role_or_size_changed") + if role in children: + if (item.get("pose_source") != IMAGE_POSE_SOURCE + or item.get("image_model_sha256") != identity): + raise ValueError("image_motion_provenance_chain_model_changed") + candidates = tuple(_pose(pose) for pose in item.get("reprojection_valid_candidates", ())) + if not candidates: + raise ValueError("image_motion_provenance_raw_candidates_missing") + else: + # A root may itself be an earlier frozen moving parent. Its + # current selected pose is used, never its previous image. + diagnostics = item.get("candidate_diagnostics", {}) + if (item.get("pose_source") not in {"verified_fixed_reference", IMAGE_POSE_SOURCE} + or diagnostics.get("branch_frozen") is not True + or diagnostics.get("branch_status") != "tracking" + or diagnostics.get("observation_stamp_ns") != frame["image_stamp_ns"]): + raise ValueError("image_motion_provenance_root_not_frozen_in_current_image") + candidates = (_pose(item["selected"]),) + points = item.get("corners_xy") + if points is not None: + array = np.asarray(points, dtype=float) + if array.shape != (4, 2) or not np.all(np.isfinite(array)): + raise ValueError("image_motion_provenance_corners_invalid") + observations.append(ImageRoleObservation(role, size, tuple(map(tuple, array)))) + elif role in children: + raise ValueError("image_motion_provenance_corners_missing") + roles.append(RoleCandidates(role, candidates, role not in children)) + result = select_image_motion_frame(model, ImageMotionFrame( + MotionEvidenceFrame(frame["image_stamp_ns"], tuple(roles)), + tuple(map(tuple, frame["camera_matrix"])), tuple(observations))) + if not result.resolved: + raise ValueError(f"image_motion_provenance_frame_unresolved:{result.reason}") + by_role = {item.role: item.pose for item in result.selections} + for role in children: + if role not in by_role or not _same_pose(_pose(frame["roles"][role]["selected"]), by_role[role]): + raise ValueError(f"image_motion_provenance_selected_pose_changed:{role}") + selected[role] = by_role[role] + return selected + + +def validate_image_sample(row, frame, replayed): + """Bind sample pose, corners, model identity and relative geometry to a frame.""" + if (row.get("view") != frame.get("view") or row.get("image_stamp_ns") != frame.get("image_stamp_ns") + or any(row.get(key) != frame.get(key) for key in ("task_name", "cycle", "direction", "attempt"))): + raise ValueError("image_motion_provenance_sample_identity_changed") + evidence = row.get("pnp_observation_evidence", {}) + if set(evidence) != {"parent", "child"}: + raise ValueError("image_motion_provenance_sample_roles_missing") + for item in evidence.values(): + role = item.get("tag_role") + data = frame["roles"].get(role) + if data is None: + raise ValueError("image_motion_provenance_sample_role_changed") + if not _same_pose(_pose(item["selected_pose"]), _pose(data["selected"])): + raise ValueError("image_motion_provenance_sample_pose_changed") + if has_image_model(item): + if (item.get("pose_source") != IMAGE_POSE_SOURCE or role not in replayed + or item.get("image_model_sha256") != data.get("image_model_sha256") + or image_model_sha256(item["frozen_image_model"]) != data.get("image_model_sha256") + or not np.array_equal(item.get("corners_xy"), data.get("corners_xy"))): + raise ValueError("image_motion_provenance_sample_model_or_corners_changed") + elif has_image_model(data): + raise ValueError("image_motion_provenance_sample_solver_marker_removed") + parent = _pose(evidence["parent"]["selected_pose"]) + child = _pose(evidence["child"]["selected_pose"]) + rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw) + relative = Rotation.from_quat(row["relative_quaternion_xyzw"]) + translation = rp.inv().apply(np.subtract(child.translation_xyz_m, parent.translation_xyz_m)) + if ((relative.inv()*rp.inv()*rc).magnitude() > 1e-7 + or not np.allclose(translation, row["relative_translation_xyz_m"], atol=1e-8, rtol=0)): + raise ValueError("image_motion_provenance_relative_pose_changed") + common_parent, common_child = row["parent_pose_common"], row["child_pose_common"] + rp = Rotation.from_quat(common_parent["quaternion_xyzw"]) + rc = Rotation.from_quat(common_child["quaternion_xyzw"]) + common_translation = rp.inv().apply(np.subtract(common_child["translation_xyz_m"], + common_parent["translation_xyz_m"])) + if ((relative.inv()*rp.inv()*rc).magnitude() > 1e-7 + or not np.allclose(translation, common_translation, atol=1e-8, rtol=0)): + raise ValueError("image_motion_provenance_common_pose_changed") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py index 6986ab3..33c4270 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py @@ -16,3 +16,4 @@ from .tag_pose.rigid_group import SquareTagGroupPoseTracker as SquareTagGroupPos from .tag_pose.parameters import PoseTrackingParameters as PoseTrackingParameters from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS as DEFAULT_POSE_TRACKING_PARAMETERS from .tag_pose.parameters import DEFAULT_REPROJECTION_TIE_PX as DEFAULT_REPROJECTION_TIE_PX +from .tag_pose.parameters import POSE_TRACKING_POLICY_VERSION as POSE_TRACKING_POLICY_VERSION diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_border.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_border.py new file mode 100644 index 0000000..a197fc5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_border.py @@ -0,0 +1,44 @@ +"""Independent pixel support for a decoded square's four outer edges. + +A partially covered AprilTag can still decode without a bit error, while its +quad follows the occluder. Such a quad is not evidence of reference motion. +This check uses the current detected quad, never the locked reference: a fully +visible translated/rotated marker remains eligible for the movement guard. +""" + +import cv2 +import numpy as np + + +def inspect_tag_border(gray, corners): + points = np.asarray(corners, dtype=np.float32) + if (gray.ndim != 2 or points.shape != (4, 2) or not np.all(np.isfinite(points))): + return {"passed": False, "reason": "border_image_or_quad_invalid"} + edge = float(np.min(np.linalg.norm(points-np.roll(points, 1, axis=0), axis=1))) + if edge < 12: + return {"passed": False, "reason": "border_image_underresolved"} + # Sample opposite sides of the printed black/white boundary, including + # near-corner portions. A wider band would miss curved occluder edges. + inset = max(.025, 1.25/edge) + u = np.linspace(.05, .95, 25, dtype=np.float32) + def bands(offset): + return np.asarray([np.c_[u, np.full(25, offset)], + np.c_[np.full(25, 1-offset), u], np.c_[u, np.full(25, 1-offset)], + np.c_[np.full(25, offset), u]], dtype=np.float32) + transform = cv2.getPerspectiveTransform(np.float32([[0, 0], [1, 0], [1, 1], [0, 1]]), points) + def sample(normalized): + xy = cv2.perspectiveTransform(normalized.reshape(-1, 1, 2), transform).reshape(4, 25, 2) + if (np.min(xy) < 1 or np.max(xy[:, :, 0]) > gray.shape[1]-2 + or np.max(xy[:, :, 1]) > gray.shape[0]-2): + return None + return cv2.remap(gray, xy[:, :, 0], xy[:, :, 1], cv2.INTER_LINEAR).astype(float) + inside, outside = sample(bands(inset)), sample(bands(-inset)) + if inside is None or outside is None: + return {"passed": False, "reason": "border_image_out_of_bounds"} + black, white = float(np.percentile(inside, 25)), float(np.percentile(outside, 75)) + threshold = (black+white)/2 + support = np.mean((inside < threshold) & (outside > threshold), axis=1) + passed = white-black >= 30 and bool(np.all(support >= .9)) + return {"passed": passed, "reason": "" if passed else "tag_border_incomplete", + "policy": "current_quad_border_v1", "edge_support": support.tolist(), + "contrast_gray": white-black, "minimum_edge_support": .9, "minimum_contrast_gray": 30} diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/branch_confirmation.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/branch_confirmation.py new file mode 100644 index 0000000..0e44e93 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/branch_confirmation.py @@ -0,0 +1,299 @@ +"""Image-evidence confirmation and frozen-reference branch consistency. + +Confirmation is repeatable image evidence, not a claim of physical truth. +After a reference is frozen, conflicting evidence must invalidate capture; +it cannot authorize a different planar solution for the same reference. +""" + +from dataclasses import dataclass +import math +from typing import Sequence + +import numpy as np + +from .ippe import rotation_distance_rad +from .fixed_reference import FixedTagReference +from .parameters import BRANCH_TRANSLATION_EQUIVALENCE_M, PoseTrackingParameters +from .types import SquareTagPose + + +@dataclass +class PendingBranch: + pose: SquareTagPose + started_ns: int + count: int = 1 + + +@dataclass +class BranchState: + observed_ns: int = 0 + pose: SquareTagPose | None = None + revision: int = 0 + pending: PendingBranch | None = None + frozen: bool = False + recovery_required: bool = False + conflict: bool = False + status: str = "initializing" + reason: str = "pose_branch_unconfirmed" + motion_evidence_id: str | None = None + + +class BranchConfirmation: + """Mutable metadata, accessed only under the owning tracker's short lock.""" + + def __init__(self, parameters: PoseTrackingParameters): + self.parameters = parameters + count = parameters.branch_confirmation_frames + if isinstance(count, bool) or not isinstance(count, int) or count < 2: + raise ValueError("branch_confirmation_frames must be an integer >= 2") + for name in ("branch_confirmation_minimum_seconds", "branch_equivalence_rad"): + value = getattr(parameters, name) + if not math.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be finite and positive") + if parameters.branch_equivalence_rad > math.radians(1.0): + raise ValueError("branch_equivalence_rad must not exceed 1 degree") + self.states: dict[str, BranchState] = {} + self.corrections: dict[str, int] = {} + + def reset(self): + self.states.clear() + self.corrections.clear() + + def distance(self, first: SquareTagPose, second: SquareTagPose) -> tuple[float, float]: + return ( + rotation_distance_rad(first.quaternion_xyzw, second.quaternion_xyzw), + float(np.linalg.norm(np.asarray(first.translation_xyz_m) - second.translation_xyz_m)), + ) + + def continuous(self, first: SquareTagPose, second: SquareTagPose) -> bool: + angle, translation = self.distance(first, second) + return (angle <= self.parameters.maximum_pose_jump_rad + and translation <= self.parameters.maximum_translation_jump_m) + + def equivalent(self, first: SquareTagPose, second: SquareTagPose) -> bool: + angle, translation = self.distance(first, second) + # A 4 cm image acceptance limit is not equivalence of two solutions. + return (angle <= self.parameters.branch_equivalence_rad + and translation <= BRANCH_TRANSLATION_EQUIVALENCE_M) + + def nearest(self, reference: SquareTagPose, candidates: Sequence[SquareTagPose]) -> SquareTagPose: + def cost(candidate): + angle, translation = self.distance(reference, candidate) + return (angle / self.parameters.maximum_pose_jump_rad + + translation / self.parameters.maximum_translation_jump_m) + return min(candidates, key=cost) + + @staticmethod + def reject(state: BranchState, reason: str, status: str) -> tuple[None, str]: + state.status, state.reason = status, reason + return None, reason + + @staticmethod + def accept(state: BranchState, selected: SquareTagPose) -> tuple[SquareTagPose, str]: + state.pose = selected + state.pending = None + state.status, state.reason = "tracking", "" + return selected, "" + + def confirmed(self, state: BranchState, selected: SquareTagPose, stamp: int, + candidates: Sequence[SquareTagPose]) -> bool: + pending = state.pending + if (pending is None or not self.continuous(pending.pose, selected) + or not self.equivalent(self.nearest(pending.pose, candidates), selected)): + state.pending = PendingBranch(selected, stamp) + return False + pending.pose = selected + pending.count += 1 + return (pending.count >= self.parameters.branch_confirmation_frames + and stamp - pending.started_ns >= round( + self.parameters.branch_confirmation_minimum_seconds * 1e9)) + + def _observe(self, role: str, stamp: int) -> tuple[BranchState | None, bool]: + state = self.states.setdefault(role, BranchState()) + if stamp <= state.observed_ns: + return None, False + gap = state.observed_ns > 0 and stamp - state.observed_ns > round( + self.parameters.reset_after_seconds * 1e9) + state.observed_ns = stamp # Rejections are observations, not a lost Tag. + return state, gap + + def update_fixed(self, role: str, reference: FixedTagReference, + checked_pose: SquareTagPose | None, stamp: int) -> tuple[SquareTagPose | None, str]: + """Validate stationarity in the image; never track a new base pose. + + The fixed pose was confirmed and frozen before this call. IPPE image + ranking alone cannot contradict the explicit stationary constraint. + A failed image invalidates observations using this reference, not the + frozen pose itself. Actual reference movement is monitored separately; + prolonged missing data is handled by the task's normal quality gates. + """ + state, gap = self._observe(role, stamp) + if state is None: + return None, "pose_observation_not_new" + if (type(reference.branch_revision) is not int or not state.frozen + or state.revision != reference.branch_revision): + return self.reject(state, "pose_branch_reference_invalid", "unavailable") + if state.conflict: + return self.reject(state, "pose_branch_conflict", "frozen_conflict") + if gap: + state.pending = None + state.recovery_required = True + if checked_pose is None: + state.pending = None + state.recovery_required = True + return self.reject(state, "fixed_reference_pose_unverified", "fixed_reference_unavailable") + if state.recovery_required: + if state.status != "recovering": + state.pending = None + if not self.confirmed(state, reference.pose, stamp, (reference.pose,)): + return self.reject(state, "pose_branch_unconfirmed", "recovering") + state.recovery_required = False + return self.accept(state, checked_pose) + + def observe_motion(self, role: str, candidates: Sequence[SquareTagPose], + stamp: int) -> tuple[None, str]: + """Register a frame whose branch will be selected jointly, outside locks. + + Recording candidates is not confirmation. In particular, repeated image + winners cannot change a motion-authorized identity while the geometric + selector is evaluating this frame. + """ + state, gap = self._observe(role, stamp) + if state is None: + return None, "pose_observation_not_new" + if state.conflict: + return self.reject(state, "pose_branch_conflict", "frozen_conflict") + if gap: + state.pending = None + if state.frozen or state.motion_evidence_id is not None: + state.recovery_required = True + else: + state.pose = None + if not candidates: + state.pending = None + return self.reject(state, "no_pose_within_reprojection_or_tilt_limit", "unavailable") + # Preserve recovery confirmation between frames; merely receiving a + # frame must not clear PendingBranch before the joint selection commits. + state.reason = "pose_motion_evidence_required" + if not state.recovery_required: + state.status = "motion_pending" + return None, state.reason + + def select_motion(self, state: BranchState, selected: SquareTagPose, + evidence_id: str, *, establish: bool) -> tuple[SquareTagPose | None, str]: + """Apply an externally verified geometric selection to one metadata copy. + + The tracker checks image membership and atomically commits all copies. + This method never fits a model or changes an existing evidence identity. + """ + if state.conflict: + return self.reject(state, "pose_branch_conflict", "frozen_conflict") + if state.motion_evidence_id is None: + if state.frozen or not establish: + return self.reject(state, "pose_motion_evidence_required", "motion_unconfirmed") + state.motion_evidence_id = evidence_id + state.revision += 1 + state.recovery_required = False + return self.accept(state, selected) + if evidence_id != state.motion_evidence_id: + return self.reject(state, "pose_motion_evidence_changed", "motion_unconfirmed") + if state.pose is None: + return self.reject(state, "pose_branch_reference_invalid", "unavailable") + if state.recovery_required: + if not self.equivalent(state.pose, selected): + state.pending = None + return self.reject(state, "pose_branch_recovery_required", "recovery_required") + if not self.confirmed(state, selected, state.observed_ns, (selected,)): + return self.reject(state, "pose_branch_unconfirmed", "recovering") + state.recovery_required = False + elif not self.continuous(state.pose, selected): + state.pending = None + return self.reject(state, "pose_motion_branch_discontinuous", "motion_unconfirmed") + return self.accept(state, selected) + + def update(self, role: str, candidates: Sequence[SquareTagPose], stamp: int, + tie: float) -> tuple[SquareTagPose | None, str]: + state, gap = self._observe(role, stamp) + if state is None: + return None, "pose_observation_not_new" + if state.conflict: + return self.reject(state, "pose_branch_conflict", "frozen_conflict") + if gap: + state.pending = None + if state.frozen: + state.recovery_required = True + else: + state.pose = None + if not candidates: + state.pending = None + return self.reject(state, "no_pose_within_reprojection_or_tilt_limit", "unavailable") + candidates = sorted(candidates, key=lambda c: c.reprojection_error_px) + best = candidates[0] + distinct = [c for c in candidates[1:] if not self.equivalent(best, c)] + decisive = not distinct or all(c.reprojection_error_px > best.reprojection_error_px + tie for c in distinct) + if state.recovery_required: + # Without a temporal path, only a return to the last known pose + # supplies a conservative reference for independent tracking. + near = self.nearest(state.pose, candidates) + if (not self.equivalent(state.pose, near) + or best.reprojection_error_px + tie < near.reprojection_error_px): + state.pending = None + return self.reject(state, "pose_branch_recovery_required", "recovery_required") + if not self.confirmed(state, near, stamp, candidates): + return self.reject(state, "pose_branch_unconfirmed", "recovering") + state.recovery_required = False + return self.accept(state, near) + if state.pose is None: + if not decisive: + state.pending = None + return self.reject(state, "pose_branch_unconfirmed", "initializing") + if not self.confirmed(state, best, stamp, candidates): + return self.reject(state, "pose_branch_unconfirmed", "initializing") + state.revision += 1 + return self.accept(state, best) + + incumbent = self.nearest(state.pose, candidates) + continuous = self.continuous(state.pose, incumbent) + advantage = best.reprojection_error_px + tie < incumbent.reprojection_error_px + if continuous and (not advantage or self.equivalent(incumbent, best)): + return self.accept(state, incumbent) + + # Propagate the incumbent's identity while evidence is rejected. This + # does not emit a sample, and cannot age into a five-second reset. + if continuous: + state.pose = incumbent + if state.frozen and continuous: + # Both image-valid branches still exist and the incumbent has a + # continuous path. A different image-error winner is ambiguity, + # not evidence that the reference moved. Quarantine the image; + # retain this path so later observations can recover without a + # reset, branch switch, or new joint zero. + state.pending = None + return self.reject(state, "pose_branch_ambiguous", "frozen_ambiguous") + if not decisive: + state.pending = None + return self.reject(state, "pose_jump", "unconfirmed") + if not self.confirmed(state, best, stamp, candidates): + return self.reject(state, "pose_branch_unconfirmed", "conflict_pending" if state.frozen else "correcting") + if state.frozen: + state.conflict = True + return self.reject(state, "pose_branch_conflict", "frozen_conflict") + state.revision += 1 + self.corrections[role] = self.corrections.get(role, 0) + 1 + self.accept(state, best) + # A corrected tracker can establish NEW references, but the transition + # itself must never be interpreted as a measured joint displacement. + return self.reject(state, "pose_branch_corrected", "corrected") + + def diagnostics(self, role: str) -> dict[str, int | str | bool]: + state = self.states[role] + return { + "observation_stamp_ns": state.observed_ns, + "branch_revision": state.revision, + "branch_status": state.status, + "branch_reason": state.reason, + "branch_confirmation_count": 0 if state.pending is None else state.pending.count, + "branch_frozen": state.frozen, + "motion_evidence_verified": state.motion_evidence_id is not None, + "motion_evidence_id": state.motion_evidence_id or "", + } diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py new file mode 100644 index 0000000..cff1135 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py @@ -0,0 +1,111 @@ +"""Image hinges constrained by geometry retained in the output URDF. + +Only adjacent parallel axes are represented here. Their signed parallelism +and perpendicular separation are invariant under joint-zero corrections. +Camera poses, Tag installations and image angles remain measured quantities. +""" + +from dataclasses import dataclass +import math + +import numpy as np +from scipy.spatial.transform import Rotation + +from .motion_image_solver import ImageHingeBundle + + +@dataclass(frozen=True) +class ParallelAxisGeometry: + parent_joint: str + child_joint: str + axis_sign: int + distance_m: float + + def __post_init__(self): + if (not self.parent_joint or not self.child_joint or self.parent_joint == self.child_joint + or self.axis_sign not in {-1, 1} + or not math.isfinite(self.distance_m) or self.distance_m <= 1e-6): + raise ValueError("invalid_parallel_axis_geometry") + + +class CadImageHingeBundle(ImageHingeBundle): + """Eliminate three variables per constrained hinge, without penalty weights. + + The child axis is the parent axis expressed in the parent Tag frame. Its + pivot is on the known-radius circle about the parent axis; one phase is + estimated. This keeps the model's axial-pivot gauge fixed and leaves no + redundant optimizer columns. The usual full Jacobian includes image-angle + uncertainty and dependencies on the parent's installation parameters. + """ + + def __init__(self, frames, relations, poses, *, constraints, source_urdf_sha256): + super().__init__(frames, relations, poses) + self.constraints = tuple(constraints) + self.source_urdf_sha256 = source_urdf_sha256 + indices = {relation.joint: i for i, relation in enumerate(relations)} + self._constraints = {} + for constraint in self.constraints: + parent = indices.get(constraint.parent_joint) + child = indices.get(constraint.child_joint) + if (parent is None or child is None or parent >= child + or relations[child].parent_role != relations[parent].child_role + or child in self._constraints): + raise ValueError("parallel_axis_constraint_outside_observed_chain") + self._constraints[child] = (parent, constraint) + self._full_initial = self.initial.copy() + removed = {10*i + local for i in self._constraints for local in (0, 1, 6)} + self._kept = np.asarray([i for i in range(len(self.initial)) if i not in removed]) + self._columns = {original: column for column, original in enumerate(self._kept)} + for index, (parent, _) in sorted(self._constraints.items()): + parameters = self._full_initial[self._kept] + _, _, _, parent_mount = self.geometry_values(parameters, parent) + _, _, old_pivot, _ = super().geometry_values(self._full_initial, index) + tangent = self.axis_basis(parameters, index) + vector = parent_mount + old_pivot + self._full_initial[10*index + 5] = math.atan2(tangent[:, 1] @ vector, tangent[:, 0] @ vector) + self.scale[10*index + 5] = 1. + self.initial = self._full_initial[self._kept] + self.scale = self.scale[self._kept] + + @property + def static_parameter_count(self): + return 10 * self.joints - 3 * len(self._constraints) + + def geometry_parameter_indices(self, index): + return tuple(self._columns[original] for original in range(10*index, 10*(index+1)) + if original in self._columns) + + def geometry_dependency_indices(self, index): + result = self.geometry_parameter_indices(index) + if index in self._constraints: + result += self.geometry_dependency_indices(self._constraints[index][0]) + return result + + def _expand(self, parameters): + full = self._full_initial.copy() + full[self._kept] = parameters + return full + + def axis_basis(self, parameters, index): + if index not in self._constraints: + values = self._expand(parameters)[10*index:10*index+2] + return Rotation.from_rotvec(self.bases[index] @ values).as_matrix() @ self.bases[index] + parent, constraint = self._constraints[index] + _, reference, _, _ = self.geometry_values(parameters, parent) + basis = reference.inv().as_matrix() @ self.axis_basis(parameters, parent) + basis[:, 1] *= constraint.axis_sign + return basis + + def geometry_values(self, parameters, index): + full = self._expand(parameters) + if index not in self._constraints: + return super().geometry_values(full, index) + parent, constraint = self._constraints[index] + parent_axis, parent_reference, _, parent_mount = self.geometry_values(parameters, parent) + axis = constraint.axis_sign * parent_reference.inv().apply(parent_axis) + values = full[10*index:10*(index+1)] + reference = Rotation.from_rotvec(values[2:5]) * self.references[index] + phase = values[5] + pivot = constraint.distance_m * (self.axis_basis(parameters, index) @ [math.cos(phase), math.sin(phase)]) + pivot -= parent_mount - axis * (axis @ parent_mount) + return axis, reference, pivot, values[7:10] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py new file mode 100644 index 0000000..746ab90 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py @@ -0,0 +1,44 @@ +"""A frozen image model with explicit, source-bound URDF constraints.""" + +from dataclasses import dataclass +from typing import ClassVar + +import numpy as np +from scipy.spatial.transform import Rotation + +from .cad_hinge import ParallelAxisGeometry +from .image_motion_model import ImageMotionModel + +CAD_IMAGE_MOTION_MODEL_POLICY = "image_hinge_v2_source_parallel_geometry" + + +@dataclass(frozen=True) +class CadImageMotionModel(ImageMotionModel): + POLICY: ClassVar[str] = CAD_IMAGE_MOTION_MODEL_POLICY + policy: str = CAD_IMAGE_MOTION_MODEL_POLICY + constraints: tuple[ParallelAxisGeometry, ...] = () + source_urdf_sha256: str = "" + + def __post_init__(self): + super().__post_init__() + if (not isinstance(self.constraints, tuple) or not self.constraints + or len(self.source_urdf_sha256) != 64 + or any(c not in "0123456789abcdef" for c in self.source_urdf_sha256)): + raise ValueError("image_model_source_geometry_missing") + geometry = {item.relation.joint: item for item in self.geometry} + children = set() + for constraint in self.constraints: + if not isinstance(constraint, ParallelAxisGeometry): + raise ValueError("image_model_source_geometry_invalid") + parent, child = geometry.get(constraint.parent_joint), geometry.get(constraint.child_joint) + if (parent is None or child is None or parent.relation.child_role != child.relation.parent_role + or constraint.child_joint in children): + raise ValueError("image_model_source_geometry_graph_changed") + children.add(constraint.child_joint) + axis = Rotation.from_quat(parent.reference_quaternion_xyzw).inv().apply(parent.axis_parent_xyz) + if not np.allclose(child.axis_parent_xyz, constraint.axis_sign * axis, atol=1e-8, rtol=0): + raise ValueError("image_model_source_axis_changed") + vector = np.asarray(parent.mount_child_xyz_m) + child.pivot_parent_xyz_m + distance = np.linalg.norm(vector - axis * (axis @ vector)) + if not np.isclose(distance, constraint.distance_m, atol=1e-8, rtol=0): + raise ValueError("image_model_source_axis_distance_changed") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/fixed_reference.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/fixed_reference.py new file mode 100644 index 0000000..4ecc8f0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/fixed_reference.py @@ -0,0 +1,11 @@ +"""An immutable camera-local pose for a declared stationary reference Tag.""" + +from dataclasses import dataclass + +from .types import SquareTagPose + + +@dataclass(frozen=True) +class FixedTagReference: + pose: SquareTagPose + branch_revision: int diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_model_selection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_model_selection.py new file mode 100644 index 0000000..fb2a29b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_model_selection.py @@ -0,0 +1,90 @@ +"""Training-only model choice and held-out, motion-stratified comparisons. + +All comparisons use squared image loss, matching the geometry optimizer. +Holm step-down controls the nominal familywise level for the complete set of +comparisons. These are conditional image tests, not a calibration certificate. +""" +import math + +import numpy as np +from scipy.stats import ttest_1samp, wilcoxon + +from .motion_evidence import MotionEvidenceParameters, _equivalent + +IMAGE_MODEL_SELECTION_POLICY = "training_squared_loss_holm_v1" + + +def training_candidate_index(hypotheses): + """Never replace a training-selected model because of validation scores.""" + eligible = [i for i, hypothesis in enumerate(hypotheses) if hypothesis.training_accepted] + if not eligible: + return None + return min(eligible, key=lambda i: float(np.mean( + [quality.rms_px ** 2 for quality in hypotheses[i].training_quality]))) + + +def equivalent_frame_selections(first, second): + if not first.resolved or not second.resolved: + return False + left = {selection.role: selection.pose for selection in first.selections} + right = {selection.role: selection.pose for selection in second.selections} + return bool(left and len(left) == len(first.selections) and len(right) == len(second.selections) + and left.keys() == right.keys() + and all(_equivalent(pose, right[role]) for role, pose in left.items())) + + +def compare_image_losses(best, alternative, angles, tolerance): + """Compare paired squared errors with the existing pixel-resolution margin. + + One observation per angular stratum prevents repeated images inflating + evidence. Medians and both mean/rank tests prevent an isolated large error + from deciding a branch. The candidate is selected on training images only. + """ + angles = np.asarray(angles, dtype=float) + best, alternative = np.asarray(best, dtype=float), np.asarray(alternative, dtype=float) + if (angles.ndim != 2 or not angles.shape[1] or best.ndim != 1 + or len(best) != len(angles) or alternative.shape != best.shape or not len(best) + or not np.all(np.isfinite(angles)) or not np.all(np.isfinite(best)) + or not np.all(np.isfinite(alternative)) or np.any(best < 0) or np.any(alternative < 0) + or not math.isfinite(tolerance) or tolerance < 0): + return 1. + moving = int(np.argmax(np.ptp(angles, axis=0))) + values = angles[:, moving] + span = float(np.ptp(values)) + parameters = MotionEvidenceParameters() + count = min(24, int(np.floor(span / parameters.angular_bin_rad))) + if count < parameters.minimum_motion_bins: + return 1. + indices = np.minimum(count - 1, np.floor((values - min(values)) / span * count).astype(int)) + losses = alternative ** 2 - (best + tolerance) ** 2 + differences = np.asarray([float(np.median(losses[indices == index])) + for index in range(count) if np.any(indices == index)]) + if (len(differences) < parameters.minimum_motion_bins + or not np.all(np.isfinite(differences)) or np.mean(differences) <= 0): + return 1. + if np.ptp(differences) < 1e-12: + return 2. ** -len(differences) if differences[0] > 0 else 1. + mean_probability = float(ttest_1samp(differences, 0., alternative="greater").pvalue) + rank_probability = float(wilcoxon(differences, alternative="greater", zero_method="wilcox").pvalue) + if not math.isfinite(mean_probability) or not math.isfinite(rank_probability): + return 1. + return max(mean_probability, rank_probability) + + +def holm_adjusted_probabilities(comparisons): + """Order-independent Holm probabilities, returned in input identity order. + + A geometric equivalence check may supply zero. It is still included in the + family; no candidate is omitted merely because of its initialization label. + Nonfinite or invalid probabilities fail closed. + """ + comparisons = tuple(comparisons) + if len({index for index, _ in comparisons}) != len(comparisons): + raise ValueError("duplicate_image_model_comparison") + values = [(index, probability if math.isfinite(probability) and 0 <= probability <= 1 else 1.) + for index, probability in comparisons] + adjusted, previous = {}, 0. + for rank, (index, probability) in enumerate(sorted(values, key=lambda item: item[1])): + previous = max(previous, min(1., probability * (len(values) - rank))) + adjusted[index] = previous + return tuple((index, adjusted[index]) for index, _ in comparisons) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py new file mode 100644 index 0000000..75498a8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py @@ -0,0 +1,157 @@ +"""Bind one image solve to versioned tracker inputs, then commit it once. + +The numerical solve runs without tracker locks. Immutable tracker-owned pixels +and frozen root snapshots prevent callers substituting observations; a weak +identity registry prevents copied or manually assembled decisions being used as +commit authority. All tracker locks are acquired only for metadata checks. +""" +from __future__ import annotations + +from contextlib import ExitStack +from collections.abc import Mapping, Sequence +from dataclasses import asdict, dataclass +import hashlib +import json +from threading import RLock +from weakref import WeakValueDictionary +from typing import TYPE_CHECKING + +from .image_motion_model import ImageMotionModel +from .image_motion_projection import select_image_motion_frame +from .image_projection_worker import ImageProjectionWorker +from .motion_image_types import ImageMotionFrame, ImageRoleQuality + +if TYPE_CHECKING: + from .tracking import MotionFrameCommit, MotionPoseSelection, MotionTrackerSnapshot + + +@dataclass(frozen=True) +class VerifiedImageMotionDecision: + accepted: bool + reason: str + selections: tuple[MotionPoseSelection, ...] = () + root_snapshots: tuple[MotionTrackerSnapshot, ...] = () + model: ImageMotionModel | None = None + model_sha256: str = "" + quality: tuple[ImageRoleQuality, ...] = () + + +_verified = WeakValueDictionary() +_registry_lock = RLock() + + +def _root_current(snapshot): + tracker, role = snapshot.tracker, snapshot.role + state = tracker._branches.states.get(role) + diagnostics = tracker.last_candidate_diagnostics_by_role.get(role, {}) + evidence_verified = (diagnostics.get("motion_selection_committed") is True and snapshot.evidence_id is not None + if snapshot.motion_managed else diagnostics.get("fixed_reference_verified") is True) + return (tracker._epoch == snapshot.epoch and state is not None + and state.observed_ns == snapshot.stamp_ns and state.revision == snapshot.revision + and state.frozen and snapshot.frozen and state.status == "tracking" + and state.pose == snapshot.previous_pose and evidence_verified + and not state.conflict and not state.recovery_required + and tracker._image_observations_by_role.get(role) is snapshot.image_observation) + + +def _inputs_match(snapshots, roots, model, frame): + items = {r.role: r for r in frame.evidence.roles} + observations = {o.role: o for o in frame.observations} + if len(items) != len(frame.evidence.roles) or len(observations) != len(frame.observations): + return False + for snapshot in (*snapshots, *roots): + image = snapshot.image_observation + item, observation = items.get(snapshot.role), observations.get(snapshot.role) + if (image is None or item is None or observation is None + or snapshot.stamp_ns != frame.evidence.stamp_ns or image.stamp_ns != snapshot.stamp_ns + or image.camera_matrix != model.camera_matrix or image.camera_matrix != frame.camera_matrix + or image.tag_size_m != observation.tag_size_m or image.corners_xy != observation.corners_xy): + return False + if snapshot in roots: + if not item.frozen or item.candidates != (snapshot.previous_pose,): + return False + elif item.frozen or item.candidates != snapshot.candidates: + return False + return True + + +def verify_image_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], model: ImageMotionModel, + frame: ImageMotionFrame, evidence_ids: Mapping[str, str], *, + root_snapshots: Sequence[MotionTrackerSnapshot] = (), + projection_worker=None) -> VerifiedImageMotionDecision: + """Calculate a decision once; never accept caller-proposed output poses.""" + from .tracking import MotionPoseSelection, SquareTagPoseTracker + if projection_worker is not None and type(projection_worker) is not ImageProjectionWorker: + return VerifiedImageMotionDecision(False, "pose_image_motion_worker_invalid") + snapshots, roots = tuple(snapshots), tuple(root_snapshots) + if not isinstance(model, ImageMotionModel) or not snapshots: + return VerifiedImageMotionDecision(False, "pose_image_motion_inputs_invalid") + child_roles = {g.relation.child_role for g in model.geometry} + root_roles = set(dict(model.tag_sizes)) - child_roles + if (set(evidence_ids) != child_roles or {s.role for s in snapshots} != child_roles + or len(snapshots) != len(child_roles) + or any(not isinstance(value, str) or not value for value in evidence_ids.values())): + return VerifiedImageMotionDecision(False, "pose_image_motion_roles_invalid") + if not roots: + collected = [] + for role in sorted(root_roles): + found = [snapshot for tracker in {s.tracker for s in snapshots} + if (snapshot := tracker.motion_snapshot(role, frame.evidence.stamp_ns)) is not None] + if len(found) != 1: + return VerifiedImageMotionDecision(False, "pose_image_motion_root_missing") + collected.append(found[0]) + roots = tuple(collected) + if {s.role for s in roots} != root_roles or len(roots) != len(root_roles): + return VerifiedImageMotionDecision(False, "pose_image_motion_root_missing") + if not _inputs_match(snapshots, roots, model, frame): + return VerifiedImageMotionDecision(False, "pose_image_motion_observation_mismatch") + with ExitStack() as stack: + for tracker in sorted({s.tracker for s in (*snapshots, *roots)}, key=id): + stack.enter_context(tracker._lock) + if not all(SquareTagPoseTracker._motion_snapshot_is_current(s) for s in snapshots) or not all( + _root_current(s) for s in roots): + return VerifiedImageMotionDecision(False, "pose_observation_superseded") + # This is the only expensive operation. Commit below never repeats it. + result = (select_image_motion_frame(model, frame) if projection_worker is None + else ImageProjectionWorker.solve(projection_worker, model, frame)) + if not result.resolved: + return VerifiedImageMotionDecision(False, result.reason, quality=result.quality) + poses = {item.role: item.pose for item in result.selections} + identity = hashlib.sha256(json.dumps(asdict(model), sort_keys=True, separators=(",", ":"), + allow_nan=False).encode()).hexdigest() + selections = tuple(MotionPoseSelection(s, poses[s.role], evidence_ids[s.role]) for s in snapshots) + decision = VerifiedImageMotionDecision(True, "", selections, roots, model, identity, result.quality) + with _registry_lock: + _verified[id(decision)] = decision + return decision + + +def commit_image_motion_frame(decision: VerifiedImageMotionDecision, *, + establish: bool = False) -> MotionFrameCommit: + """Consume a verifier-created decision and atomically update all children.""" + from .tracking import MotionFrameCommit, SquareTagPoseTracker + if not isinstance(decision, VerifiedImageMotionDecision): + return MotionFrameCommit(False, "pose_image_motion_decision_invalid") + if not decision.accepted: + return MotionFrameCommit(False, decision.reason) + with _registry_lock: + if _verified.pop(id(decision), None) is not decision: + return MotionFrameCommit(False, "pose_image_motion_decision_invalid") + snapshots = tuple(item.snapshot for item in decision.selections) + with ExitStack() as stack: + for tracker in sorted({s.tracker for s in (*snapshots, *decision.root_snapshots)}, key=id): + stack.enter_context(tracker._lock) + if not all(SquareTagPoseTracker._motion_snapshot_is_current(s) for s in snapshots) or not all( + _root_current(s) for s in decision.root_snapshots): + return MotionFrameCommit(False, "pose_observation_superseded") + for snapshot in snapshots: + tracker, role = snapshot.tracker, snapshot.role + if tracker._image_observations_by_role.get(role) is not snapshot.image_observation: + return MotionFrameCommit(False, "pose_image_motion_observation_mismatch") + previous = tracker._image_model_sha256_by_role.get(role) + if previous is not None and previous != decision.model_sha256: + return MotionFrameCommit(False, "pose_image_motion_model_changed") + if previous is None and snapshot.evidence_id is not None: + return MotionFrameCommit(False, "pose_motion_solver_changed") + return SquareTagPoseTracker._commit_motion_selections_locked(decision.selections, + establish=establish, image_decision=decision) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py new file mode 100644 index 0000000..6a68cf3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py @@ -0,0 +1,136 @@ +"""Frozen image-derived hinge models and their auditable authorization evidence.""" +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import ClassVar, Mapping + +import numpy as np + +from .motion_evidence import MotionRelation, MotionRoleSelection +from .motion_image_types import ImageHingeGeometry, ImageRoleQuality +from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS + +IMAGE_MOTION_MODEL_POLICY = "image_hinge_v1_pre_zero_frozen" + + +@dataclass(frozen=True) +class ImageMotionModel: + POLICY: ClassVar[str] = IMAGE_MOTION_MODEL_POLICY + geometry: tuple[ImageHingeGeometry, ...] + camera_matrix: tuple[tuple[float, float, float], ...] + tag_sizes: tuple[tuple[str, float], ...] + source_stamps_ns: tuple[int, ...] + training_stamps_ns: tuple[int, ...] + validation_stamps_ns: tuple[int, ...] + branches: tuple[tuple[str, int], ...] + maximum_reprojection_error_px: float = 1.5 + reprojection_tie_px: float = .03 + policy: str = IMAGE_MOTION_MODEL_POLICY + + @property + def relations(self): + return tuple(g.relation for g in self.geometry) + + def __post_init__(self): + from .motion_image_diagnostics import _ordered_relations + if self.policy != self.POLICY: + raise ValueError("image_motion_model_policy_mismatch") + sequences = (self.geometry, self.camera_matrix, self.tag_sizes, self.source_stamps_ns, + self.training_stamps_ns, self.validation_stamps_ns, self.branches) + if any(not isinstance(value, tuple) for value in sequences) or any( + not isinstance(value, tuple) for values in (self.camera_matrix, self.tag_sizes, self.branches) + for value in values): + raise ValueError("image_motion_model_requires_immutable_sequences") + relations = self.relations + if tuple(_ordered_relations(relations)) != relations: + raise ValueError("image_motion_model_graph_not_ordered") + p = DEFAULT_POSE_TRACKING_PARAMETERS + if (self.maximum_reprojection_error_px != p.maximum_reprojection_error_px + or self.reprojection_tie_px != p.reprojection_tie_px): + raise ValueError("image_motion_model_quality_policy_changed") + matrix = np.asarray(self.camera_matrix) + if (matrix.shape != (3, 3) or not np.all(np.isfinite(matrix)) + or matrix[0, 0] <= 0 or matrix[1, 1] <= 0 + or not np.array_equal(matrix[2], [0, 0, 1])): + raise ValueError("image_motion_model_camera_invalid") + roles = {r for g in self.geometry for r in (g.relation.parent_role, g.relation.child_role)} + if (len(dict(self.tag_sizes)) != len(self.tag_sizes) or set(dict(self.tag_sizes)) != roles + or any(not math.isfinite(s) or s <= 0 for _, s in self.tag_sizes)): + raise ValueError("image_motion_model_tag_sizes_invalid") + if len(dict(self.branches)) != len(self.branches) or set(dict(self.branches)) != roles or any( + type(v) is not int or v not in (0, 1) for _, v in self.branches): + raise ValueError("image_motion_model_branches_invalid") + for stamps in (self.source_stamps_ns, self.training_stamps_ns, self.validation_stamps_ns): + if not stamps or any(type(v) is not int or v <= 0 for v in stamps) or any( + b <= a for a, b in zip(stamps, stamps[1:])): + raise ValueError("image_motion_model_sources_invalid") + source, train, validation = map(set, (self.source_stamps_ns, self.training_stamps_ns, + self.validation_stamps_ns)) + if not train.isdisjoint(validation) or not train | validation <= source: + raise ValueError("image_motion_model_source_overlap") + for geometry in self.geometry: + vectors = (geometry.axis_parent_xyz, geometry.reference_quaternion_xyzw, + geometry.pivot_parent_xyz_m, geometry.mount_child_xyz_m) + if any(not isinstance(v, tuple) or len(v) != n for v, n in zip(vectors, (3, 4, 3, 3))) or not all( + math.isfinite(x) for v in vectors for x in v): + raise ValueError("image_motion_model_geometry_invalid") + if not np.isclose(np.linalg.norm(vectors[0]), 1., atol=1e-7) or not np.isclose( + np.linalg.norm(vectors[1]), 1., atol=1e-7): + raise ValueError("image_motion_model_rotation_invalid") + + +@dataclass(frozen=True) +class ImageMotionFrameSelection: + resolved: bool + reason: str + selections: tuple[MotionRoleSelection, ...] = () + quality: tuple[ImageRoleQuality, ...] = () + angles_rad: tuple[tuple[str, float], ...] = () + + +@dataclass(frozen=True) +class ImageMotionHypothesis: + branches: tuple[tuple[str, int], ...] + converged: bool + reason: str + training_quality: tuple[ImageRoleQuality, ...] = () + validation_quality: tuple[ImageRoleQuality, ...] = () + motion_span_rad: tuple[tuple[str, float], ...] = () + axis_uncertainty_95_rad: tuple[tuple[str, float], ...] = () + pivot_uncertainty_95_m: tuple[tuple[str, float], ...] = () + comparison_p_values: tuple[tuple[int, float], ...] = () + training_solver_status: int | None = None + training_solver_evaluations: int = 0 + training_accepted: bool = False + comparison_adjusted_p_values: tuple[tuple[int, float], ...] = () + + +@dataclass(frozen=True) +class ImageMotionResolution: + resolved: bool + reason: str + model: ImageMotionModel | None = None + hypotheses: tuple[ImageMotionHypothesis, ...] = () + selections: tuple[MotionRoleSelection, ...] = () + + +def image_motion_model_from_dict(raw: Mapping) -> ImageMotionModel: + """Read an immutable model; hashes/provenance are enforced by the caller.""" + geometry = tuple(ImageHingeGeometry(MotionRelation(**item["relation"]), + tuple(item["axis_parent_xyz"]), tuple(item["reference_quaternion_xyzw"]), + tuple(item["pivot_parent_xyz_m"]), tuple(item["mount_child_xyz_m"])) for item in raw["geometry"]) + model_type, extra = ImageMotionModel, {} + from .cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY, CadImageMotionModel + if raw["policy"] == CAD_IMAGE_MOTION_MODEL_POLICY: + from .cad_hinge import ParallelAxisGeometry + model_type = CadImageMotionModel + extra = {"constraints": tuple(ParallelAxisGeometry(**value) for value in raw["constraints"]), + "source_urdf_sha256": raw["source_urdf_sha256"]} + return model_type(geometry=geometry, camera_matrix=tuple(tuple(v) for v in raw["camera_matrix"]), + tag_sizes=tuple((str(r), float(s)) for r, s in raw["tag_sizes"]), + source_stamps_ns=tuple(raw["source_stamps_ns"]), training_stamps_ns=tuple(raw["training_stamps_ns"]), + validation_stamps_ns=tuple(raw["validation_stamps_ns"]), + branches=tuple((str(r), i) for r, i in raw["branches"]), + maximum_reprojection_error_px=raw["maximum_reprojection_error_px"], + reprojection_tie_px=raw["reprojection_tie_px"], policy=raw["policy"], **extra) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py new file mode 100644 index 0000000..a75cbcc --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py @@ -0,0 +1,131 @@ +"""Current-image angle estimates with immutable, pre-zero hinge geometry.""" +from itertools import product + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation + +from .image_motion_model import ImageMotionFrameSelection +from .ippe import square_object_points +from .motion_evidence import MotionEvidenceParameters, MotionRoleSelection, _eligible, _equivalent +from .motion_image_diagnostics import _quality +from .relative import _relative_pose +from .types import SquareTagPose + + +def project_image_model(model, angles, roots): + transforms = {role: (Rotation.from_quat(pose.quaternion_xyzw), + np.array(pose.translation_xyz_m)) for role, pose in roots.items()} + pixels = [] + positive = True + matrix, sizes = np.array(model.camera_matrix), dict(model.tag_sizes) + for geometry, angle in zip(model.geometry, angles): + relative = Rotation.from_rotvec(np.array(geometry.axis_parent_xyz) * angle) * Rotation.from_quat( + geometry.reference_quaternion_xyzw) + parent_rotation, parent_translation = transforms[geometry.relation.parent_role] + rotation = parent_rotation * relative + translation = parent_translation + parent_rotation.apply(np.array(geometry.pivot_parent_xyz_m) + + relative.apply(geometry.mount_child_xyz_m)) + role = geometry.relation.child_role + transforms[role] = (rotation, translation) + points = rotation.apply(square_object_points(sizes[role])) + translation + positive = positive and bool(np.all(points[:, 2] > 1e-6)) + homogeneous = points @ matrix.T + pixels.append(homogeneous[:, :2] / np.maximum(homogeneous[:, 2:], 1e-6)) + return np.asarray(pixels), transforms, positive + + +def _frame_inputs(model, frame): + if tuple(tuple(v) for v in frame.camera_matrix) != model.camera_matrix: + raise ValueError("image_motion_camera_changed") + observations = {o.role: o for o in frame.observations} + candidates = {r.role: r for r in frame.evidence.roles} + roles, sizes = tuple(dict(model.tag_sizes)), dict(model.tag_sizes) + if (len(observations) != len(frame.observations) or len(candidates) != len(frame.evidence.roles) + or not set(roles) <= observations.keys() or not set(roles) <= candidates.keys()): + raise ValueError("image_motion_frame_roles_missing") + for role in roles: + observation = observations[role] + if observation.tag_size_m != sizes[role] or np.shape(observation.corners_xy) != (4, 2) or not np.all( + np.isfinite(observation.corners_xy)): + raise ValueError("image_motion_frame_corners_invalid") + children = tuple(g.relation.child_role for g in model.geometry) + roots = set(roles) - set(children) + poses = {} + for role in roles: + selected = tuple(p for p in candidates[role].candidates if _eligible(p)) + if not selected or (role in roots and (not candidates[role].frozen or len(selected) != 1)): + raise ValueError("image_motion_current_root_or_candidates_unavailable") + poses[role] = selected + frozen = {role: poses[role][0] for role in roots} + # A frozen root is still verified against this image, never silently moved. + for role, pose in frozen.items(): + xyz = Rotation.from_quat(pose.quaternion_xyzw).apply(square_object_points(sizes[role])) + xyz += pose.translation_xyz_m + if np.any(xyz[:, 2] <= 1e-6): + raise ValueError("image_motion_root_behind_camera") + image = xyz @ np.array(model.camera_matrix).T + delta = image[:, :2] / image[:, 2:] - observations[role].corners_xy + if np.sqrt(np.mean(np.sum(delta ** 2, axis=1))) > model.maximum_reprojection_error_px: + raise ValueError("image_motion_current_root_reprojection_failed") + return poses, frozen, np.asarray([observations[r].corners_xy for r in children]), children + + +def select_image_motion_frame(model, frame): + """Fit only current geometric angles, retaining all candidate initializations. + + No model parameter, baseline or previous pose is changed. Distinct solutions + within the public 0.03 pixel tolerance are rejected rather than ranked. + """ + try: + poses, roots, observed, children = _frame_inputs(model, frame) + roles = tuple(poses) + if np.prod([len(poses[r]) for r in roles]) > 256: + raise ValueError("image_motion_frame_hypothesis_limit") + fits = [] + for values in product(*(poses[role] for role in roles)): + selected = dict(zip(roles, values)) + initial = [] + for geometry in model.geometry: + rotation, _ = _relative_pose(selected[geometry.relation.parent_role], + selected[geometry.relation.child_role]) + initial.append(float((rotation * Rotation.from_quat( + geometry.reference_quaternion_xyzw).inv()).as_rotvec() @ geometry.axis_parent_xyz)) + def residual(angles): + pixels, _, _ = project_image_model(model, angles, roots) + return (pixels - observed).ravel() + fit = least_squares(residual, initial, max_nfev=40, ftol=1e-9, xtol=1e-9, gtol=1e-7) + _, transforms, positive = project_image_model(model, fit.x, roots) + if not fit.success or not positive or not np.all(np.isfinite(fit.fun)): + raise ValueError("image_motion_frame_optimizer_unresolved") + quality = _quality(fit.fun, children) + output = dict(roots) + for role, checked in zip(children, quality): + rotation, translation = transforms[role] + output[role] = SquareTagPose(tuple(rotation.as_quat()), tuple(translation), checked.rms_px) + score = float(np.sqrt(np.mean(np.square(fit.fun.reshape(-1, 2)).sum(axis=1)))) + fits.append((score, output, quality, fit.x, fit.jac, fit.fun)) + if not fits: + raise ValueError("image_motion_frame_reprojection_or_tilt_failed") + fits.sort(key=lambda item: item[0]) + best = fits[0] + for other in fits[1:]: + if other[0] <= best[0] + model.reprojection_tie_px and not all( + _equivalent(best[1][r], other[1][r]) for r in roles): + raise ValueError("image_motion_frame_angles_ambiguous") + jacobian = np.asarray(best[4]) + _, singular, vh = np.linalg.svd(jacobian, full_matrices=False) + if singular[-1] <= singular[0] * np.finfo(float).eps * max(jacobian.shape): + raise ValueError("image_motion_frame_angles_unobservable") + variance = max(model.reprojection_tie_px ** 2, + float(np.sum(best[5] ** 2) / max(1, len(best[5]) - len(best[3])))) + covariance = (vh.T / singular ** 2) @ vh * variance + if np.any(3 * np.sqrt(np.maximum(0., np.diag(covariance))) > + MotionEvidenceParameters().maximum_frame_off_axis_rad): + raise ValueError("image_motion_frame_angles_uncertain") + accepted = all(_eligible(p) for p in best[1].values()) + return ImageMotionFrameSelection(accepted, "" if accepted else "image_motion_frame_reprojection_or_tilt_failed", + tuple(MotionRoleSelection(r, best[1][r]) for r in roles), best[2], + tuple((g.relation.joint, float(a)) for g, a in zip(model.geometry, best[3]))) + except (ValueError, TypeError, KeyError, FloatingPointError, np.linalg.LinAlgError) as error: + return ImageMotionFrameSelection(False, str(error)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_projection_worker.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_projection_worker.py new file mode 100644 index 0000000..2938fdd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_projection_worker.py @@ -0,0 +1,38 @@ +"""Bounded process execution of the unchanged, pure current-image solver.""" + +from concurrent.futures import ProcessPoolExecutor, TimeoutError +from concurrent.futures.process import BrokenProcessPool +import multiprocessing +import threading + +from .image_motion_model import ImageMotionFrameSelection +from .image_motion_projection import select_image_motion_frame + + +class ImageProjectionWorker: + def __init__(self, workers=3): + self._pool = ProcessPoolExecutor(max_workers=workers, + mp_context=multiprocessing.get_context("spawn")) + self._slots = threading.BoundedSemaphore(workers) + self._closed = False + + def solve(self, model, frame): + if self._closed or not self._slots.acquire(blocking=False): + return ImageMotionFrameSelection(False, "image_motion_frame_worker_busy") + try: + future = self._pool.submit(select_image_motion_frame, model, frame) + except (RuntimeError, BrokenProcessPool): + self._slots.release() + return ImageMotionFrameSelection(False, "image_motion_frame_worker_unavailable") + future.add_done_callback(lambda _: self._slots.release()) + try: + return future.result(timeout=2.) + except TimeoutError: + future.cancel() + return ImageMotionFrameSelection(False, "image_motion_frame_worker_timeout") + except BrokenProcessPool: + return ImageMotionFrameSelection(False, "image_motion_frame_worker_unavailable") + + def close(self): + self._closed = True + self._pool.shutdown(wait=False, cancel_futures=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py index f7da3a9..2c2a302 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py @@ -111,6 +111,33 @@ def solve_square_tag_ippe( return candidates +def fixed_pose_reprojection_error( + pose: SquareTagPose, + corners_xy: Sequence[Sequence[float]], + *, + tag_size_m: float, + camera_matrix: Sequence[Sequence[float]], +) -> float: + """Check a frozen pose against rectified corners without solving a new pose.""" + corners = np.asarray(corners_xy, dtype=np.float64) + if corners.shape != (4, 2) or not np.all(np.isfinite(corners)): + raise ValueError("corners_xy must contain four finite image points") + intrinsic = _as_camera_matrix(camera_matrix) + points = square_object_points(tag_size_m) + rotation = Rotation.from_quat(pose.quaternion_xyzw) + translation = np.asarray(pose.translation_xyz_m, dtype=np.float64) + camera_points = rotation.apply(points) + translation + if not np.all(np.isfinite(camera_points)) or np.min(camera_points[:, 2]) <= 0: + raise ValueError("fixed pose must have finite, positive-depth corners") + projected, _ = cv2.projectPoints(points, rotation.as_rotvec(), translation, + intrinsic, np.zeros(4, dtype=np.float64)) + residual = projected.reshape(4, 2) - corners + error = float(np.sqrt(np.mean(np.sum(residual * residual, axis=1)))) + if not math.isfinite(error): + raise ValueError("fixed pose reprojection error must be finite") + return error + + def rotation_distance_rad( first_xyzw: Sequence[float], second_xyzw: Sequence[float], diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py new file mode 100644 index 0000000..65f5540 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py @@ -0,0 +1,413 @@ +"""Pre-freeze planar-branch evidence from a single articulated motion. + +The measurements are actual image candidates, never SDK-derived angles. A +hinge has one rotation axis and one fixed pivot in its parent Tag coordinates: +``R = exp(theta * axis) R_ref`` and ``t = pivot + R * mount``. These are the +same fixed-axis/fixed-pivot relations used by candidate_selection's trajectory +score. Unlike that diagnostic ranking, this module requires a unique hypothesis +to pass absolute quality and observability checks before authorizing a branch. + +The 1.5 mm / 1 degree RMS scales come from that existing trajectory score. +Motion span, cross-validation and conditioning gates are additional evidence +requirements, not changes to final calibration accuracy limits. Calibration +holdout and previously frozen references must never enter this fit API. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from itertools import permutations, product +import math +from typing import Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS +from .relative import _relative_pose +from .types import SquareTagPose + + +@dataclass(frozen=True) +class MotionRelation: + joint: str + parent_role: str + child_role: str + + +@dataclass(frozen=True) +class RoleCandidates: + """Actual candidates; ``frozen`` means already selected by prior evidence. + + A previously frozen moving parent must be selected in this image using its + existing constraint before a new relation is trained. Its last image's pose + is not a substitute. Raw current-frame roots passed to select_motion_frame + remain a candidate set; the tracker's separate frozen flag is not this flag. + """ + role: str + candidates: tuple[SquareTagPose, ...] + frozen: bool = False + + +@dataclass(frozen=True) +class MotionEvidenceFrame: + stamp_ns: int + roles: tuple[RoleCandidates, ...] + + +@dataclass(frozen=True) +class MotionRoleSelection: + role: str + pose: SquareTagPose + + +@dataclass(frozen=True) +class MotionEvidenceParameters: + minimum_motion_rad: float = math.radians(15.0) + angular_bin_rad: float = math.radians(1.0) + minimum_motion_bins: int = 12 + minimum_translation_span_m: float = 0.003 + maximum_pivot_condition: float = 100.0 + maximum_pivot_rms_m: float = 0.0015 + maximum_pivot_p95_m: float = 0.003 + maximum_off_axis_rms_rad: float = math.radians(1.0) + maximum_off_axis_p95_rad: float = math.radians(2.0) + maximum_split_axis_difference_rad: float = math.radians(3.0) + maximum_frame_off_axis_rad: float = math.radians(3.0) + maximum_frame_pivot_error_m: float = 0.003 + + def __post_init__(self): + for name, value in vars(self).items(): + if not math.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be finite and positive") + if type(self.minimum_motion_bins) is not int or self.minimum_motion_bins < 12: + raise ValueError("minimum_motion_bins must be an integer >= 12") + + +@dataclass(frozen=True) +class MotionConstraintResidual: + off_axis_rad: float + pivot_error_m: float + accepted: bool + + +@dataclass(frozen=True) +class FrozenMotionConstraint: + relation: MotionRelation + reference_quaternion_xyzw: tuple[float, float, float, float] + axis_parent_xyz: tuple[float, float, float] + pivot_parent_xyz_m: tuple[float, float, float] + mount_child_xyz_m: tuple[float, float, float] + source_stamps_ns: tuple[int, ...] + maximum_off_axis_rad: float + maximum_pivot_error_m: float + + def evaluate(self, parent: SquareTagPose, child: SquareTagPose) -> MotionConstraintResidual: + rotation, translation = _relative_pose(parent, child) + off_axis, pivot = _residuals(self, Rotation.from_quat([rotation.as_quat()]), + np.asarray([translation])) + a, p = float(off_axis[0]), float(pivot[0]) + return MotionConstraintResidual(a, p, bool( + _eligible(parent) and _eligible(child) + and a <= self.maximum_off_axis_rad and p <= self.maximum_pivot_error_m)) + + +@dataclass(frozen=True) +class MotionRelationQuality: + joint: str + reason: str + motion_rad: float = 0.0 + motion_bins: int = 0 + translation_span_m: float = 0.0 + pivot_condition: float = 0.0 + pivot_rms_m: float = 0.0 + pivot_p95_m: float = 0.0 + off_axis_rms_rad: float = 0.0 + off_axis_p95_rad: float = 0.0 + split_axis_difference_rad: float = 0.0 + + +@dataclass(frozen=True) +class MotionHypothesis: + branches: tuple[tuple[str, int], ...] + relations: tuple[MotionRelationQuality, ...] + accepted: bool + + +@dataclass(frozen=True) +class MotionBranchResolution: + resolved: bool + reason: str + selections: tuple[MotionRoleSelection, ...] = () + constraints: tuple[FrozenMotionConstraint, ...] = () + hypotheses: tuple[MotionHypothesis, ...] = () + + +@dataclass(frozen=True) +class MotionFrameSelection: + resolved: bool + reason: str + selections: tuple[MotionRoleSelection, ...] = () + + +def _eligible(pose: SquareTagPose) -> bool: + values = (*pose.quaternion_xyzw, *pose.translation_xyz_m, pose.reprojection_error_px) + if (len(pose.quaternion_xyzw) != 4 or len(pose.translation_xyz_m) != 3 + or not np.all(np.isfinite(values)) + or not np.isclose(np.linalg.norm(pose.quaternion_xyzw), 1.0, atol=1e-5) + or pose.translation_xyz_m[2] <= 0 + or not 0 <= pose.reprojection_error_px <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px): + return False + normal = Rotation.from_quat(pose.quaternion_xyzw).apply([0, 0, 1]) + tilt = math.acos(float(np.clip(abs(normal[2]), 0, 1))) + return tilt <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad + + +def _distance(first: SquareTagPose, second: SquareTagPose) -> tuple[float, float]: + return (float((Rotation.from_quat(first.quaternion_xyzw).inv() + * Rotation.from_quat(second.quaternion_xyzw)).magnitude()), + float(np.linalg.norm(np.subtract(first.translation_xyz_m, second.translation_xyz_m)))) + + +def _equivalent(first: SquareTagPose, second: SquareTagPose) -> bool: + angle, distance = _distance(first, second) + return angle <= math.radians(1.0) and distance <= 0.001 + + +def _validate_relations(relations: Sequence[MotionRelation]) -> tuple[str, ...]: + if not relations or len({r.joint for r in relations}) != len(relations): + raise ValueError("motion relations need non-empty unique joints") + roles = tuple(dict.fromkeys(role for r in relations for role in (r.parent_role, r.child_role))) + if len(roles) > 8 or any(not r.joint or not r.parent_role or not r.child_role + or r.parent_role == r.child_role for r in relations): + raise ValueError("motion relations need a bounded articulated graph") + return roles + + +def _candidate_map(items: Sequence[RoleCandidates], roles: Sequence[str]): + result = {item.role: item for item in items} + if len(result) != len(items) or any(role not in result for role in roles): + return None + for role in roles: + item = result[role] + if not 1 <= len(item.candidates) <= 2 or (item.frozen and len(item.candidates) != 1): + return None + valid = tuple(candidate for candidate in item.candidates if _eligible(candidate)) + if not valid: + return None + result[role] = RoleCandidates(role, valid, item.frozen) + return result + + +def _connect_paths(frames: Sequence[MotionEvidenceFrame], roles: Sequence[str]): + """Preserve both continuous families without treating continuity as truth.""" + candidates = [_candidate_map(frame.roles, roles) for frame in frames] + if any(item is None for item in candidates): + return None, "motion_candidates_missing_or_invalid" + paths = {} + for role in roles: + items = [frame[role] for frame in candidates] + if any(item.frozen != items[0].frozen for item in items): + return None, "motion_candidate_family_interrupted" + # Anchor where both eligible roots are available. If an alternative + # temporarily fails the image-quality gate it stays missing; never copy + # the surviving root into both families to invent evidence. + anchor = max(range(len(items)), key=lambda i: len(items[i].candidates)) + count = len(items[anchor].candidates) + families = [[None] * len(frames) for _ in range(count)] + for index, candidate in enumerate(items[anchor].candidates): + families[index][anchor] = candidate + for sequence in (range(anchor + 1, len(frames)), range(anchor - 1, -1, -1)): + previous = list(items[anchor].candidates) + for frame_index in sequence: + current = items[frame_index].candidates + def cost(order): + return sum(_distance(previous[family], candidate)[0] + for family, candidate in zip(order, current)) + orders = sorted(permutations(range(count), len(current)), key=cost) + if (len(orders) == 2 and abs(cost(orders[0]) - cost(orders[1])) < 1e-9 + and not _equivalent(previous[0], previous[1])): + return None, "motion_candidate_assignment_ambiguous" + for family, candidate in zip(orders[0], current): + angle, translation = _distance(previous[family], candidate) + p = DEFAULT_POSE_TRACKING_PARAMETERS + if angle > p.maximum_pose_jump_rad or translation > p.maximum_translation_jump_m: + return None, "motion_candidate_family_discontinuous" + families[family][frame_index] = candidate + previous[family] = candidate + paths[role] = families + # Near-coincident IPPE roots represent one physical hypothesis, not two. + for role, families in paths.items(): + if len(families) == 2 and all(a is not None and b is not None and _equivalent(a, b) + for a, b in zip(*families)): + paths[role] = families[:1] + return paths, "" + + +def _axis(rotations: Rotation) -> tuple[np.ndarray, np.ndarray]: + vectors = (rotations * rotations[0].inv()).as_rotvec() + axis = np.linalg.svd(vectors, full_matrices=False)[2][0] + if np.dot(axis, vectors[-1]) < 0: + axis = -axis + return axis, vectors @ axis + + +def _fit_constraint(relation, rotations, points, stamps, parameters): + axis, angles = _axis(rotations) + projected = Rotation.from_rotvec(angles[:, None] * axis) * rotations[0] + # One axial pivot gauge is unobservable for any revolute joint. Projecting + # onto the measured rotation axis gives rank five instead of fitting that + # gauge to tiny orientation noise with an arbitrary sixth parameter. + design = np.concatenate((np.tile(np.eye(3), (len(points), 1, 1)), + projected.as_matrix()), axis=2).reshape(-1, 6) + u, singular, vh = np.linalg.svd(design, full_matrices=False) + condition = float(singular[0] / max(singular[4], np.finfo(float).tiny)) + solution = vh[:5].T @ ((u[:, :5].T @ points.ravel()) / singular[:5]) + model = FrozenMotionConstraint(relation, tuple(rotations[0].as_quat()), tuple(axis), + tuple(solution[:3]), tuple(solution[3:]), tuple(stamps), + parameters.maximum_frame_off_axis_rad, parameters.maximum_frame_pivot_error_m) + return model, condition + + +def _residuals(model: FrozenMotionConstraint, rotations: Rotation, points: np.ndarray): + delta = rotations * Rotation.from_quat(model.reference_quaternion_xyzw).inv() + axis = np.asarray(model.axis_parent_xyz) + projected = Rotation.from_rotvec((delta.as_rotvec() @ axis)[:, None] * axis) + angular = (projected.inv() * delta).magnitude() + predicted = np.asarray(model.pivot_parent_xyz_m) + rotations.apply(model.mount_child_xyz_m) + return angular, np.linalg.norm(points - predicted, axis=1) + + +def _metrics(angular, pivot): + return (float(np.sqrt(np.mean(pivot**2))), float(np.percentile(pivot, 95)), + float(np.sqrt(np.mean(angular**2))), float(np.percentile(angular, 95))) + + +def _passes(values, parameters): + return all(value <= limit for value, limit in zip(values, ( + parameters.maximum_pivot_rms_m, parameters.maximum_pivot_p95_m, + parameters.maximum_off_axis_rms_rad, parameters.maximum_off_axis_p95_rad))) + + +def _relation_evidence(relation, parent, child, stamps, parameters): + supported = [index for index, (p, c) in enumerate(zip(parent, child)) if p is not None and c is not None] + if len(supported) < parameters.minimum_motion_bins: + return None, MotionRelationQuality(joint=relation.joint, reason="insufficient_candidate_support") + pairs = [_relative_pose(parent[index], child[index]) for index in supported] + stamps = tuple(stamps[index] for index in supported) + rotations = Rotation.from_quat([rotation.as_quat() for rotation, _ in pairs]) + points = np.asarray([point for _, point in pairs]) + _, angles = _axis(rotations) + motion = float(np.ptp(angles)) + # Use one actual image per angular bin; stationary endpoints and high frame + # rates cannot inflate the evidence count or overwhelm the moving arc. + bins: dict[int, list[int]] = {} + for index, angle in enumerate(angles): + key = int(np.floor((angle - float(np.min(angles))) / parameters.angular_bin_rad + 1e-9)) + bins.setdefault(key, []).append(index) + indices = sorted(values[len(values) // 2] for values in bins.values()) + span = float(np.linalg.norm(np.ptp(points[indices], axis=0))) + common = dict(joint=relation.joint, motion_rad=motion, motion_bins=len(indices), translation_span_m=span) + if motion < parameters.minimum_motion_rad or len(indices) < parameters.minimum_motion_bins: + return None, MotionRelationQuality(reason="insufficient_rotation_evidence", **common) + if motion >= math.radians(170): + return None, MotionRelationQuality(reason="rotation_log_wrap_unsupported", **common) + if span < parameters.minimum_translation_span_m: + return None, MotionRelationQuality(reason="insufficient_pivot_translation", **common) + rotations, points = rotations[indices], points[indices] + selected_stamps = tuple(stamps[index] for index in indices) + model, condition = _fit_constraint(relation, rotations, points, selected_stamps, parameters) + values = _metrics(*_residuals(model, rotations, points)) + quality = dict(common, pivot_condition=condition, pivot_rms_m=values[0], pivot_p95_m=values[1], + off_axis_rms_rad=values[2], off_axis_p95_rad=values[3]) + if condition > parameters.maximum_pivot_condition: + return None, MotionRelationQuality(reason="ill_conditioned_pivot", **quality) + if not _passes(values, parameters): + return None, MotionRelationQuality(reason="motion_geometry_residual", **quality) + # Check interleaved bins and separate early/late intervals in both + # directions. Dense or temporally correlated samples cannot independently + # confirm each other just by being adjacent. This is pre-freeze evidence + # only; no fourth calibration round is consumed here. + count = len(indices) + splits = ((np.arange(0, count, 2), np.arange(1, count, 2)), + (np.arange(count // 2), np.arange(count // 2, count))) + maximum_difference = 0.0 + for first, second in splits: + models = [] + for train, validate in ((first, second), (second, first)): + split, split_condition = _fit_constraint(relation, rotations[train], points[train], + tuple(selected_stamps[i] for i in train), parameters) + models.append(split) + if (split_condition > parameters.maximum_pivot_condition + or not _passes(_metrics(*_residuals(split, rotations[validate], points[validate])), parameters)): + return None, MotionRelationQuality(reason="motion_split_validation_failed", **quality) + difference = math.acos(float(np.clip(abs(np.dot(models[0].axis_parent_xyz, + models[1].axis_parent_xyz)), 0, 1))) + maximum_difference = max(maximum_difference, difference) + quality["split_axis_difference_rad"] = maximum_difference + if maximum_difference > parameters.maximum_split_axis_difference_rad: + return None, MotionRelationQuality(reason="motion_split_axis_inconsistent", **quality) + return model, MotionRelationQuality(reason="", **quality) + + +def resolve_motion_branches( + frames: Sequence[MotionEvidenceFrame], relations: Sequence[MotionRelation], *, + parameters: MotionEvidenceParameters = MotionEvidenceParameters(), +) -> MotionBranchResolution: + """Resolve only a unique geometrically admissible pre-freeze hypothesis. + + The caller authorizes this input as the current session's declared zero + approach, before any affected joint is frozen. A previously frozen role is + represented by its sole already-verified candidate, never both IPPE roots. + """ + roles = _validate_relations(relations) + if len(frames) < parameters.minimum_motion_bins: + return MotionBranchResolution(False, "motion_evidence_insufficient_frames") + stamps = tuple(frame.stamp_ns for frame in frames) + if any(type(stamp) is not int or stamp < 0 for stamp in stamps) or any( + a >= b for a, b in zip(stamps, stamps[1:])): + return MotionBranchResolution(False, "motion_evidence_image_identity_invalid") + paths, reason = _connect_paths(frames, roles) + if paths is None: + return MotionBranchResolution(False, reason) + hypotheses, accepted = [], [] + for branches in product(*(range(len(paths[role])) for role in roles)): + selected = {role: paths[role][branch] for role, branch in zip(roles, branches)} + models, qualities = [], [] + for relation in relations: + model, quality = _relation_evidence(relation, selected[relation.parent_role], + selected[relation.child_role], stamps, parameters) + qualities.append(quality) + if model is not None: + models.append(model) + valid = len(models) == len(relations) and all(selected[role][-1] is not None for role in roles) + hypotheses.append(MotionHypothesis(tuple(zip(roles, branches)), tuple(qualities), valid)) + if valid: + accepted.append((selected, tuple(models))) + if len(accepted) != 1: + reason = "motion_geometry_ambiguous" if accepted else "motion_geometry_unresolved" + return MotionBranchResolution(False, reason, hypotheses=tuple(hypotheses)) + selected, models = accepted[0] + return MotionBranchResolution(True, "", tuple(MotionRoleSelection(role, selected[role][-1]) + for role in roles), models, tuple(hypotheses)) + + +def select_motion_frame( + constraints: Sequence[FrozenMotionConstraint], current_candidates: Sequence[RoleCandidates], +) -> MotionFrameSelection: + """Apply immutable evidence to this frame; never refit or synthesize poses.""" + roles = _validate_relations(tuple(model.relation for model in constraints)) + items = _candidate_map(current_candidates, roles) + if items is None: + return MotionFrameSelection(False, "motion_candidates_missing_or_invalid") + accepted = [] + for combination in product(*(items[role].candidates for role in roles)): + poses = dict(zip(roles, combination)) + if not all(model.evaluate(poses[model.relation.parent_role], poses[model.relation.child_role]).accepted + for model in constraints): + continue + if not any(all(_equivalent(poses[role], previous[role]) for role in roles) for previous in accepted): + accepted.append(poses) + if len(accepted) != 1: + return MotionFrameSelection(False, "motion_geometry_ambiguous" if accepted else "motion_geometry_inconsistent") + return MotionFrameSelection(True, "", tuple(MotionRoleSelection(role, accepted[0][role]) for role in roles)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py new file mode 100644 index 0000000..f1cceb5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py @@ -0,0 +1,223 @@ +"""Image-domain articulated motion diagnosis, never branch authorization. + +Each candidate family keeps its own fit. Frozen geometry is evaluated on held +out images by solving image angles only. No SDK values, calibration zero or +fourth-round fitting data enter this API. Callers must retain source provenance +and run this bounded but expensive analysis outside runtime state locks. +""" +from __future__ import annotations + +from itertools import product +from typing import Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from .motion_evidence import ( + MotionEvidenceParameters, MotionRelation, _axis, _connect_paths, _fit_constraint, _residuals, +) +from .motion_image_solver import ImageHingeBundle +from .motion_image_types import ( + ImageAxisRepeatability, ImageHypothesisDiagnostic, ImageIntervalDiagnostic, + ImageMotionFrame, ImageRoleObservation, ImageRoleQuality, MotionImageDiagnosticParameters, + MotionImageDiagnosticReport, PnpHingeDiagnostic, +) +from .relative import _relative_pose + +__all__ = ["ImageMotionFrame", "ImageRoleObservation", "MotionImageDiagnosticParameters", + "MotionImageDiagnosticReport", "analyze_motion_images"] + + +def _ordered_relations(relations): + if not relations or len({r.joint for r in relations}) != len(relations): + raise ValueError("relations_missing_or_duplicate_joint") + children = {r.child_role for r in relations} + if len(children) != len(relations) or any(r.parent_role == r.child_role for r in relations): + raise ValueError("multiple_parent_or_self_relation") + known = {r.parent_role for r in relations} - children + pending, ordered = list(relations), [] + while pending: + available = [r for r in pending if r.parent_role in known] + if not available: + raise ValueError("cyclic_motion_graph") + for relation in available: + ordered.append(relation) + known.add(relation.child_role) + pending.remove(relation) + return tuple(ordered) + + +def _validate_frames(frames, relations, limits): + if len(frames) < limits.minimum_frames: + raise ValueError("insufficient_image_frames") + if len(frames) > 512: + raise ValueError("source_frame_limit_exceeded") + stamps = [f.evidence.stamp_ns for f in frames] + if any(type(stamp) is not int or stamp <= 0 for stamp in stamps) or any( + newer <= older for older, newer in zip(stamps, stamps[1:])): + raise ValueError("image_identity_invalid") + roles = tuple(dict.fromkeys(role for r in relations for role in (r.parent_role, r.child_role))) + roots = set(roles) - {r.child_role for r in relations} + sizes = {} + first_matrix = np.asarray(frames[0].camera_matrix) + for frame in frames: + matrix = np.asarray(frame.camera_matrix) + if (matrix.shape != (3, 3) or not np.all(np.isfinite(matrix)) + or matrix[0, 0] <= 0 or matrix[1, 1] <= 0 + or not np.array_equal(matrix[2], [0, 0, 1])): + raise ValueError("camera_matrix_invalid") + if not np.array_equal(matrix, first_matrix): + raise ValueError("camera_model_changed") + observations = {o.role: o for o in frame.observations} + candidates = {r.role: r for r in frame.evidence.roles} + if len(observations) != len(frame.observations) or any(role not in observations for role in roles): + raise ValueError("image_observation_missing_or_duplicate") + if any(role not in candidates or not candidates[role].frozen for role in roots): + raise ValueError("root_pose_not_frozen") + for role in roles: + observation = observations[role] + corners = np.asarray(observation.corners_xy) + size = observation.tag_size_m + if (corners.shape != (4, 2) or not np.all(np.isfinite(corners)) + or not np.isfinite(size) or size <= 0): + raise ValueError("tag_geometry_invalid") + if role in sizes and size != sizes[role]: + raise ValueError("tag_size_changed") + sizes[role] = size + paths, reason = _connect_paths([f.evidence for f in frames], roles) + if paths is None: + raise ValueError(reason) + return paths, roles + + +def _sample(indices, maximum): + if len(indices) <= maximum: + return list(indices) + return [indices[i] for i in np.linspace(0, len(indices) - 1, maximum, dtype=int)] + + +def _quality(residuals, roles): + norms = np.linalg.norm(np.asarray(residuals).reshape(-1, len(roles), 4, 2), axis=-1) + result = [] + for index, role in enumerate(roles): + rms = np.sqrt(np.mean(norms[:, index] ** 2, axis=1)) + result.append(ImageRoleQuality(role, len(rms), float(np.sqrt(np.mean(rms ** 2))), + float(np.percentile(rms, 95)), float(np.max(rms)))) + return tuple(result) + + +def _subset(poses, indices): + return {role: tuple(items[i] for i in indices) for role, items in poses.items()} + + +def _interval(name, frames, relations, poses, train, validation, limits): + train = _sample(train, limits.maximum_training_frames) + validation = _sample(validation, limits.maximum_validation_frames) + stamps = tuple(frames[i].evidence.stamp_ns for i in train) + heldout = tuple(frames[i].evidence.stamp_ns for i in validation) + if len(train) < limits.minimum_frames or not heldout: + return ImageIntervalDiagnostic(name, False, "insufficient_interval_frames", 0, stamps, heldout) + try: + bundle = ImageHingeBundle(tuple(frames[i] for i in train), relations, _subset(poses, train)) + fit = bundle.solve(limits) + residuals, valid = bundle.validate(fit.x, tuple(frames[i] for i in validation), + _subset(poses, validation), limits) + training = _quality(fit.fun, bundle.child_roles) + checked = _quality(residuals, bundle.child_roles) + converged = bool(fit.success and valid and np.all(np.isfinite(fit.fun)) + and np.all(np.isfinite(residuals))) + return ImageIntervalDiagnostic(name, converged, "" if converged else "optimizer_or_projection_unresolved", + int(fit.nfev), stamps, heldout, training, checked, bundle.geometry(fit.x), + tuple(bundle.motion_spans)) + except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: + return ImageIntervalDiagnostic(name, False, str(error), 0, stamps, heldout) + + +def _pnp_diagnostics(frames, relations, poses): + result = [] + for relation in relations: + parents, children = poses[relation.parent_role], poses[relation.child_role] + pairs = [_relative_pose(p, c) for p, c in zip(parents, children)] + rotations = Rotation.from_quat([r.as_quat() for r, _ in pairs]) + points = np.asarray([t for _, t in pairs]) + _, angles = _axis(rotations) + span = float(np.ptp(angles)) + if span < MotionEvidenceParameters().minimum_motion_rad: + raise ValueError("insufficient_rotation_evidence") + if span >= np.deg2rad(170): + raise ValueError("rotation_log_wrap_unsupported") + model, condition = _fit_constraint(relation, rotations, points, + tuple(f.evidence.stamp_ns for f in frames), MotionEvidenceParameters()) + if not np.isfinite(condition) or condition > MotionEvidenceParameters().maximum_pivot_condition: + raise ValueError("ill_conditioned_pivot") + angular, errors = _residuals(model, rotations, points) + difference = points - np.asarray(model.pivot_parent_xyz_m) - rotations.apply(model.mount_child_xyz_m) + camera_difference = Rotation.from_quat([p.quaternion_xyzw for p in parents]).apply(difference) + total_squared = float(np.sum(camera_difference ** 2)) + depth_fraction = float(np.sum(camera_difference[:, 2] ** 2) / total_squared) if total_squared > 1e-24 else 0.0 + result.append(PnpHingeDiagnostic(relation.joint, span, + float(np.sqrt(np.mean(errors ** 2))), float(np.percentile(errors, 95)), + depth_fraction, float(np.sqrt(np.mean(angular ** 2))))) + return tuple(result) + + +def _hypothesis(frames, relations, poses, branches, limits): + supported = [i for i in range(len(frames)) if all(items[i] is not None for items in poses.values())] + frames, poses = tuple(frames[i] for i in supported), _subset(poses, supported) + try: + pnp = _pnp_diagnostics(frames, relations, poses) if len(frames) >= limits.minimum_frames else () + if not pnp: + return ImageHypothesisDiagnostic(branches, "insufficient_candidate_support") + except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: + return ImageHypothesisDiagnostic(branches, str(error)) + full = list(range(len(frames))) + train = _sample(full[::2], limits.maximum_training_frames) + heldout = [i for i in full if i not in set(train)] + midpoint = len(frames) // 2 + intervals = ( + _interval("interleaved", frames, relations, poses, train, heldout, limits), + _interval("early_to_late", frames, relations, poses, full[:midpoint], full[midpoint:], limits), + _interval("late_to_early", frames, relations, poses, full[midpoint:], full[:midpoint], limits), + ) + repeatability = [] + early, late = intervals[1:] + if early.converged and late.converged: + for first, second in zip(early.geometry, late.geometry): + dot = abs(float(np.dot(first.axis_parent_xyz, second.axis_parent_xyz))) + repeatability.append(ImageAxisRepeatability(first.relation.joint, + float(np.arccos(np.clip(dot, -1, 1))), + float(np.linalg.norm(np.subtract(first.pivot_parent_xyz_m, second.pivot_parent_xyz_m))))) + reason = "" if all(interval.converged for interval in intervals) else "incomplete_interval_analysis" + return ImageHypothesisDiagnostic(branches, reason, pnp, intervals, tuple(repeatability)) + + +def analyze_motion_images(frames: Sequence[ImageMotionFrame], relations: Sequence[MotionRelation], *, + parameters: MotionImageDiagnosticParameters | None = None) -> MotionImageDiagnosticReport: + """Return diagnostic evidence only; never select a winning branch or pose. + + Maximum work is bounded by source frames, graph size, candidate families, + sampled image counts and optimizer evaluation limits. Roots must already be + frozen; calibration holdout images must never be passed as training frames. + """ + limits = parameters or MotionImageDiagnosticParameters() + frames, relations = tuple(frames), tuple(relations) + stamps = tuple(frame.evidence.stamp_ns for frame in frames) + try: + if len(relations) > limits.maximum_relations: + raise ValueError("motion_graph_complexity_limit") + relations = _ordered_relations(relations) + paths, roles = _validate_frames(frames, relations, limits) + family_count = int(np.prod([len(paths[role]) for role in roles])) + if family_count > limits.maximum_hypotheses: + raise ValueError("candidate_hypothesis_limit") + except (ValueError, TypeError) as error: + return MotionImageDiagnosticReport("not_available", str(error), stamps) + hypotheses = [] + for indices in product(*(range(len(paths[role])) for role in roles)): + branches = tuple(zip(roles, indices)) + poses = {role: paths[role][index] for role, index in branches} + hypotheses.append(_hypothesis(frames, relations, poses, branches, limits)) + complete = all(not h.reason for h in hypotheses) + status = "available" if complete else "partial" if any(h.intervals for h in hypotheses) else "not_available" + return MotionImageDiagnosticReport(status, "" if complete else "some_hypotheses_not_evaluable", + stamps, tuple(hypotheses)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py new file mode 100644 index 0000000..f60a4e9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py @@ -0,0 +1,174 @@ +"""Bounded articulated image fit for preparation and offline diagnostics.""" +from __future__ import annotations + +import numpy as np +from scipy.optimize import least_squares +from scipy.sparse import lil_matrix +from scipy.spatial.transform import Rotation + +from .ippe import square_object_points +from .motion_evidence import MotionEvidenceParameters, _axis, _fit_constraint +from .relative import _relative_pose +from .motion_image_types import ImageHingeGeometry + + +def _basis(axis): + other = np.array([1., 0, 0]) if abs(axis[0]) < .9 else np.array([0., 1, 0]) + first = np.cross(axis, other) + first /= np.linalg.norm(first) + return np.stack((first, np.cross(axis, first)), axis=1) + + +class ImageHingeBundle: + """Shared hinge geometry and per-frame geometric angles; no SDK inputs. + + Relations are topologically sorted. Roots are actual, already frozen poses + for each image, so a previously frozen moving parent is supported too. + Every child has one parent. Each hinge removes the unobservable axial pivot + gauge explicitly. Validation may change angles only, never shared geometry. + """ + + def __init__(self, frames, relations, poses): + self.frames, self.relations, self.poses = frames, relations, poses + self.count, self.joints = len(frames), len(relations) + self.child_roles = tuple(r.child_role for r in relations) + self.root_roles = tuple(dict.fromkeys(r.parent_role for r in relations + if r.parent_role not in self.child_roles)) + self.axes, self.bases, self.references, angles, initial = [], [], [], [], [] + self.motion_spans = [] + for relation in relations: + pairs = [_relative_pose(parent, child) for parent, child in + zip(poses[relation.parent_role], poses[relation.child_role])] + rotations = Rotation.from_quat([r.as_quat() for r, _ in pairs]) + points = np.asarray([t for _, t in pairs]) + axis, theta = _axis(rotations) + # The full arc has passed the diagnostic minimum-span check. A + # half interval is only a counterfactual fit: retain its measured + # span in the report rather than treating it as new authorization. + span = float(np.ptp(theta)) + if span < 1e-6: + raise ValueError("insufficient_interval_rotation") + self.motion_spans.append((relation.joint, span)) + model, condition = _fit_constraint(relation, rotations, points, + tuple(f.evidence.stamp_ns for f in frames), MotionEvidenceParameters()) + if not np.isfinite(condition) or condition > MotionEvidenceParameters().maximum_pivot_condition: + raise ValueError("ill_conditioned_interval_pivot") + pivot, mount = np.array(model.pivot_parent_xyz_m), np.array(model.mount_child_xyz_m) + axial = float(pivot @ axis) + pivot -= axial * axis + mount += rotations[0].inv().apply(axis) * axial + basis = _basis(axis) + self.axes.append(axis) + self.bases.append(basis) + self.references.append(rotations[0]) + angles.append(theta) + initial.extend((0., 0., 0., 0., 0., *(basis.T @ pivot), *mount)) + self.initial = np.r_[initial, np.asarray(angles).T[1:].ravel()] + self.scale = np.r_[np.tile([1, 1, 1, 1, 1, .05, .05, .05, .05, .05], self.joints), + np.ones(self.joints * (self.count - 1))] + self.root_transforms, self.matrices, self.observed, self.objects = self._image_inputs(frames, poses) + + @property + def static_parameter_count(self): + return 10 * self.joints + + def geometry_parameter_indices(self, index): + return tuple(range(index * 10, (index + 1) * 10)) + + def geometry_dependency_indices(self, index): + return self.geometry_parameter_indices(index) + + def _image_inputs(self, frames, poses): + roots = {role: (Rotation.from_quat([p.quaternion_xyzw for p in poses[role]]), + np.asarray([p.translation_xyz_m for p in poses[role]])) for role in self.root_roles} + observations = [{o.role: o for o in frame.observations} for frame in frames] + observed = np.asarray([[o[role].corners_xy for role in self.child_roles] for o in observations]) + objects = {role: square_object_points(observations[0][role].tag_size_m) for role in self.child_roles} + return roots, np.asarray([f.camera_matrix for f in frames]), observed, objects + + def geometry_values(self, parameters, index): + values = parameters[index * 10:(index + 1) * 10] + change = Rotation.from_rotvec(self.bases[index] @ values[:2]) + axis = change.apply(self.axes[index]) + reference = Rotation.from_rotvec(values[2:5]) * self.references[index] + # Transport the initial tangent basis with the axis. Rebuilding + # _basis(axis) switches its helper at |axis.x|=.9, which changes the + # meaning of the same pivot coordinates discontinuously during a fit. + pivot = change.apply(self.bases[index] @ values[5:7]) + return axis, reference, pivot, values[7:10] + + def geometry(self, parameters): + result = [] + for index, relation in enumerate(self.relations): + axis, reference, pivot, mount = self.geometry_values(parameters, index) + result.append(ImageHingeGeometry(relation, tuple(axis), tuple(reference.as_quat()), + tuple(pivot), tuple(mount))) + return tuple(result) + + def project(self, parameters, angles, roots, matrices, objects): + transforms = dict(roots) + projected, valid = [], True + for index, relation in enumerate(self.relations): + axis, reference, pivot, mount = self.geometry_values(parameters, index) + relative = Rotation.from_rotvec(angles[:, index, None] * axis) * reference + parent_rotation, parent_translation = transforms[relation.parent_role] + translation = parent_translation + parent_rotation.apply(pivot + relative.apply(mount)) + rotation = parent_rotation * relative + transforms[relation.child_role] = (rotation, translation) + camera_points = np.einsum('nij,kj->nki', rotation.as_matrix(), objects[relation.child_role]) + camera_points += translation[:, None, :] + valid = valid and bool(np.all(camera_points[:, :, 2] > 1e-6)) + pixels = np.einsum('nij,nkj->nki', matrices, camera_points) + depth = np.maximum(pixels[:, :, 2:], 1e-6) + projected.append(pixels[:, :, :2] / depth) + return np.stack(projected, axis=1), valid + + def residual(self, parameters): + angles = np.vstack((np.zeros(self.joints), parameters[self.static_parameter_count:].reshape(-1, self.joints))) + pixels, _ = self.project(parameters, angles, self.root_transforms, self.matrices, self.objects) + return (pixels - self.observed).ravel() + + def solve(self, parameters): + # A child's image depends on its ancestors' parameters and angles only. + ancestry = {} + for index, relation in enumerate(self.relations): + ancestry[relation.child_role] = (*ancestry.get(relation.parent_role, ()), index) + width = 8 * self.joints + sparsity = lil_matrix((self.count * width, len(self.initial)), dtype=int) + for frame in range(self.count): + for child, role in enumerate(self.child_roles): + rows = slice(frame * width + child * 8, frame * width + (child + 1) * 8) + for ancestor in ancestry[role]: + sparsity[rows, self.geometry_parameter_indices(ancestor)] = 1 + if frame: + sparsity[rows, self.static_parameter_count + (frame - 1) * self.joints + ancestor] = 1 + return least_squares(self.residual, self.initial, jac_sparsity=sparsity.tocsr(), + x_scale=self.scale, max_nfev=parameters.maximum_solver_evaluations, + # LSMR's default 1e-6 inner tolerance leaves coupled hinge/angle + # steps too approximate near the image minimum. Tighten that + # linear solve so the unchanged outer convergence tolerances can + # be reached within the bounded evaluation budget. + tr_options={"atol": 1e-10, "btol": 1e-10}, + ftol=1e-9, xtol=1e-9, gtol=1e-7) + + def validate(self, parameters, frames, poses, limits): + roots, matrices, observed, objects = self._image_inputs(frames, poses) + residuals, converged = [], True + for frame in range(len(frames)): + guess = [] + for index, relation in enumerate(self.relations): + rotation, _ = _relative_pose(poses[relation.parent_role][frame], poses[relation.child_role][frame]) + axis, reference, _, _ = self.geometry_values(parameters, index) + guess.append(float((rotation * reference.inv()).as_rotvec() @ axis)) + frame_roots = {role: (r[frame:frame + 1], t[frame:frame + 1]) for role, (r, t) in roots.items()} + def residual(angles): + pixels, _ = self.project(parameters, angles.reshape(1, self.joints), frame_roots, + matrices[frame:frame + 1], objects) + return (pixels[0] - observed[frame]).ravel() + fit = least_squares(residual, guess, max_nfev=limits.maximum_frame_evaluations, + ftol=1e-9, xtol=1e-9, gtol=1e-7) + _, positive_depth = self.project(parameters, fit.x.reshape(1, self.joints), frame_roots, + matrices[frame:frame + 1], objects) + converged = converged and bool(fit.success and positive_depth) + residuals.append(fit.fun.reshape(self.joints, 4, 2)) + return np.asarray(residuals), converged diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py new file mode 100644 index 0000000..fc5e79f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py @@ -0,0 +1,121 @@ +"""Immutable inputs and reports for image-domain motion diagnostics. + +These types carry no branch authorization, zero reference or calibration map. +Image residuals and metric PnP residuals deliberately have different fields. +""" +from __future__ import annotations + +from dataclasses import dataclass, field + +from .motion_evidence import MotionEvidenceFrame, MotionRelation + + +@dataclass(frozen=True) +class ImageRoleObservation: + role: str + tag_size_m: float + corners_xy: tuple[tuple[float, float], ...] + + +@dataclass(frozen=True) +class ImageMotionFrame: + evidence: MotionEvidenceFrame + camera_matrix: tuple[tuple[float, float, float], ...] + observations: tuple[ImageRoleObservation, ...] + + +@dataclass(frozen=True) +class MotionImageDiagnosticParameters: + maximum_training_frames: int = 36 + maximum_validation_frames: int = 72 + maximum_solver_evaluations: int = 80 + maximum_frame_evaluations: int = 25 + maximum_relations: int = 3 + maximum_hypotheses: int = 8 + minimum_frames: int = 12 + + def __post_init__(self): + for name, value in vars(self).items(): + if type(value) is not int or value <= 0: + raise ValueError(f"{name} must be a positive integer") + if self.minimum_frames < 12 or self.maximum_training_frames < self.minimum_frames: + raise ValueError("image diagnostics require at least 12 training frames") + if (self.maximum_training_frames > 72 or self.maximum_validation_frames > 144 + or self.maximum_solver_evaluations > 150 or self.maximum_frame_evaluations > 50 + or self.maximum_relations > 3 or self.maximum_hypotheses > 8): + raise ValueError("image diagnostic computation limit exceeded") + + +@dataclass(frozen=True) +class ImageRoleQuality: + role: str + frames: int + rms_px: float + frame_rms_p95_px: float + maximum_frame_rms_px: float + + +@dataclass(frozen=True) +class ImageHingeGeometry: + relation: MotionRelation + axis_parent_xyz: tuple[float, float, float] + reference_quaternion_xyzw: tuple[float, float, float, float] + pivot_parent_xyz_m: tuple[float, float, float] + mount_child_xyz_m: tuple[float, float, float] + + +@dataclass(frozen=True) +class ImageIntervalDiagnostic: + name: str + converged: bool + reason: str + solver_evaluations: int + training_stamps_ns: tuple[int, ...] + validation_stamps_ns: tuple[int, ...] + training_quality: tuple[ImageRoleQuality, ...] = () + validation_quality: tuple[ImageRoleQuality, ...] = () + geometry: tuple[ImageHingeGeometry, ...] = () + training_motion_span_rad_by_joint: tuple[tuple[str, float], ...] = () + + +@dataclass(frozen=True) +class PnpHingeDiagnostic: + joint: str + motion_span_rad: float + translation_rms_m: float + translation_p95_m: float + camera_depth_squared_error_fraction: float + off_axis_rms_rad: float + + +@dataclass(frozen=True) +class ImageAxisRepeatability: + joint: str + early_late_axis_difference_rad: float + early_late_pivot_difference_m: float + + +@dataclass(frozen=True) +class ImageHypothesisDiagnostic: + branches: tuple[tuple[str, int], ...] + reason: str + pnp_geometry: tuple[PnpHingeDiagnostic, ...] = () + intervals: tuple[ImageIntervalDiagnostic, ...] = () + repeatability: tuple[ImageAxisRepeatability, ...] = () + + +@dataclass(frozen=True) +class MotionImageDiagnosticReport: + status: str + reason: str + source_stamps_ns: tuple[int, ...] + hypotheses: tuple[ImageHypothesisDiagnostic, ...] = () + scope: str = field(default="diagnostic_not_branch_authorization", init=False) + branch_authorization_allowed: bool = field(default=False, init=False) + calibration_accuracy_verified: bool = field(default=False, init=False) + interpretation: str = ( + "Pixel residuals test image consistency; PnP metric residuals test noisy pose estimates. " + "Neither lowest pixel score nor this diagnostic replaces the existing branch, " + "baseline, interval-repeatability or independent calibration holdout gates. " + "The first training angle is a solver gauge, not a joint zero reference." + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py index 37177e1..dfa4d85 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py @@ -5,6 +5,10 @@ from __future__ import annotations from dataclasses import dataclass import math + +BRANCH_TRANSLATION_EQUIVALENCE_M = 0.001 + + @dataclass(frozen=True) class PoseTrackingParameters: """One shared set of defaults, with angles stored in radians.""" @@ -15,7 +19,17 @@ class PoseTrackingParameters: maximum_translation_jump_m: float = 0.04 maximum_tag_tilt_rad: float = math.radians(75.0) reset_after_seconds: float = 5.0 + branch_confirmation_frames: int = 5 + branch_confirmation_minimum_seconds: float = 0.1 + branch_equivalence_rad: float = math.radians(1.0) DEFAULT_POSE_TRACKING_PARAMETERS = PoseTrackingParameters() DEFAULT_REPROJECTION_TIE_PX = DEFAULT_POSE_TRACKING_PARAMETERS.reprojection_tie_px +POSE_TRACKING_POLICY_VERSION = "confirmed_branch_v8_training_model_selection" +FROZEN_BRANCH_POLICY_VERSIONS = frozenset({ + "confirmed_branch_v1", "confirmed_branch_v2_fixed_reference", + "confirmed_branch_v3_recoverable_ambiguity", "confirmed_branch_v5_image_motion", + "confirmed_branch_v6_source_geometry", "confirmed_branch_v7_final_image_sampling", + POSE_TRACKING_POLICY_VERSION, +}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py new file mode 100644 index 0000000..8a5ce84 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py @@ -0,0 +1,230 @@ +"""Pre-zero motion authorization from images, followed by frozen image geometry. + +A hinge bundle minimizes measured corner errors, not the poorly conditioned +single-image PnP depth estimates. All IPPE families seed independent fits. A +model is authorized only with observable geometry, absolute image quality and +held-out, motion-stratified evidence separating every distinct alternative. +This preparation-image check does not replace final fourth-round acceptance. +""" +from dataclasses import dataclass, replace +from itertools import product + +import numpy as np + +from .image_motion_model import ( + IMAGE_MOTION_MODEL_POLICY, ImageMotionModel, ImageMotionResolution, + ImageMotionHypothesis, ImageMotionFrameSelection, image_motion_model_from_dict, +) +from .image_motion_projection import select_image_motion_frame +from .motion_evidence import MotionEvidenceParameters +from .image_model_selection import ( + compare_image_losses as _comparison, equivalent_frame_selections, + holm_adjusted_probabilities, training_candidate_index, +) +from .motion_image_diagnostics import ( + _ordered_relations, _validate_frames, _sample, _subset, _quality, _pnp_diagnostics, +) +from .motion_image_solver import ImageHingeBundle + +__all__ = ["ImageMotionModel", "ImageMotionResolution", "ImageMotionFrameSelection", + "resolve_image_motion", "select_image_motion_frame", "image_motion_model_from_dict", + "IMAGE_MOTION_MODEL_POLICY"] + + +@dataclass(frozen=True) +class ImageMotionParameters: + maximum_training_frames: int = 72 + maximum_validation_frames: int = 72 + maximum_solver_evaluations: int = 150 + maximum_frame_evaluations: int = 40 + minimum_frames: int = 24 + maximum_hypotheses: int = 64 + maximum_relations: int = 6 + familywise_error_probability: float = .01 + + def __post_init__(self): + for name in ("maximum_training_frames", "maximum_validation_frames", "maximum_solver_evaluations", + "maximum_frame_evaluations", "minimum_frames", "maximum_hypotheses", "maximum_relations"): + if type(getattr(self, name)) is not int or getattr(self, name) <= 0: + raise ValueError(f"{name} must be a positive integer") + if (self.minimum_frames < 24 or self.maximum_training_frames < 12 + or self.maximum_training_frames > 72 or self.maximum_validation_frames > 144 + or self.maximum_solver_evaluations > 500 or self.maximum_frame_evaluations > 100 + or self.maximum_hypotheses > 256 or self.maximum_relations > 8 + or not 0 < self.familywise_error_probability <= .01): + raise ValueError("image_motion_limits_invalid") + + +def _uncertainty(bundle, fit): + """Gaussian local confidence after estimating all per-image nuisance angles. + + The bundle already fixes angle and axial-pivot gauges. SVD of the full + Jacobian accounts for coupling to per-frame angles. Residual variance has a + 0.03 pixel floor, the existing image branch resolution, to avoid claiming + infinite precision on synthetic or rounded input. Three-sigma conservative + vector bounds cover the axis's two and pivot's three coordinates at >95%. + """ + jacobian = fit.jac.toarray() if hasattr(fit.jac, "toarray") else np.asarray(fit.jac) + scaled = jacobian * bundle.scale[None, :] + _, singular, vh = np.linalg.svd(scaled, full_matrices=False) + if singular[-1] <= singular[0] * np.finfo(float).eps * max(scaled.shape): + raise ValueError("image_motion_geometry_rank_deficient") + sigma_squared = max(.03 ** 2, float(np.sum(fit.fun ** 2) / max(1, len(fit.fun) - len(fit.x)))) + covariance_scaled = (vh.T / singular ** 2) @ vh * sigma_squared + covariance = covariance_scaled * bundle.scale[:, None] * bundle.scale[None, :] + axes, pivots = [], [] + for index, relation in enumerate(bundle.relations): + derivative = np.zeros((6, len(fit.x))) + for column in bundle.geometry_dependency_indices(index): + step = 1e-5 * bundle.scale[column] + upper, lower = fit.x.copy(), fit.x.copy() + upper[column] += step + lower[column] -= step + ua, _, up, _ = bundle.geometry_values(upper, index) + la, _, lp, _ = bundle.geometry_values(lower, index) + derivative[:, column] = np.r_[ua - la, up - lp] / (2 * step) + propagated = derivative @ covariance @ derivative.T + axes.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[:3, :3])[-1]))))) + pivots.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[3:, 3:])[-1]))))) + return tuple(axes), tuple(pivots) + + +def _model(bundle, fit, frames, branches, training, validation): + from .cad_hinge import CadImageHingeBundle + model_type, extra = ImageMotionModel, {} + if isinstance(bundle, CadImageHingeBundle): + from .cad_image_model import CadImageMotionModel + model_type = CadImageMotionModel + extra = {"constraints": bundle.constraints, "source_urdf_sha256": bundle.source_urdf_sha256} + return model_type(bundle.geometry(fit.x), tuple(tuple(v) for v in frames[0].camera_matrix), + tuple((role, next(o.tag_size_m for o in frames[0].observations if o.role == role)) + for role, _ in branches), tuple(f.evidence.stamp_ns for f in frames), + tuple(frames[i].evidence.stamp_ns for i in training), + tuple(frames[i].evidence.stamp_ns for i in validation), branches, **extra) + + +def _fit_hypothesis(frames, relations, poses, branches, training, validation, limits, + *, geometry_constraints=(), source_urdf_sha256=""): + if any(pose is None for role in poses.values() for pose in role): + return ImageMotionHypothesis(branches, False, "incomplete_candidate_family"), None, None + try: + pnp = _pnp_diagnostics(frames, relations, poses) + selected = tuple(frames[i] for i in training) + if geometry_constraints: + from .cad_hinge import CadImageHingeBundle + bundle = CadImageHingeBundle(selected, relations, _subset(poses, training), + constraints=geometry_constraints, source_urdf_sha256=source_urdf_sha256) + else: + bundle = ImageHingeBundle(selected, relations, _subset(poses, training)) + fit = bundle.solve(limits) + model = _model(bundle, fit, frames, branches, training, validation) + training_quality = _quality(fit.fun, bundle.child_roles) + training_converged = bool(fit.success and np.all(np.isfinite(fit.fun))) + axes, pivots = _uncertainty(bundle, fit) if training_converged else ((), ()) + p = MotionEvidenceParameters() + reason = "" if training_converged else "image_motion_optimizer_unresolved" + if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in training_quality): + reason = "image_motion_reprojection_failed" + if not reason and (any(v > p.maximum_split_axis_difference_rad for _, v in axes) + or any(v > p.maximum_frame_pivot_error_m for _, v in pivots)): + reason = "image_motion_geometry_uncertain" + training_accepted = not reason + try: + residuals, valid = bundle.validate(fit.x, tuple(frames[i] for i in validation), + _subset(poses, validation), limits) + validation_quality = _quality(residuals, bundle.child_roles) + except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: + # Keep training eligibility: a failed holdout must veto the chosen + # candidate, never silently make the next candidate the winner. + residuals, valid, validation_quality = None, False, () + reason = reason or str(error) + converged = bool(training_converged and valid and np.all(np.isfinite(residuals))) + if not reason and not converged: + reason = "image_motion_optimizer_unresolved" + if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in validation_quality): + reason = "image_motion_reprojection_failed" + hypothesis = ImageMotionHypothesis(branches, converged, reason, training_quality, validation_quality, + tuple((r.joint, r.motion_span_rad) for r in pnp), axes, pivots, + training_solver_status=int(fit.status), training_solver_evaluations=int(fit.nfev), + training_accepted=training_accepted) + rms = None if residuals is None else np.sqrt(np.mean(np.sum(residuals ** 2, axis=-1), axis=(1, 2))) + return hypothesis, model, rms + except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: + return ImageMotionHypothesis(branches, False, str(error)), None, None + + +def resolve_image_motion(frames, relations, *, parameters=None, geometry_constraints=(), source_urdf_sha256=""): + """Only pre-zero preparation frames may enter this expensive training API. + + Runtime owns session, motion and phase checks. This function knows no SDK + channels, baseline or round and cannot acquire extra training images itself. + Its output model retains the disjoint image identities needed for auditing. + """ + limits = parameters or ImageMotionParameters() + frames, relations = tuple(frames), tuple(relations) + try: + relations = _ordered_relations(relations) + if len(relations) > limits.maximum_relations: + raise ValueError("image_motion_graph_limit") + paths, roles = _validate_frames(frames, relations, limits) + if np.prod([len(paths[r]) for r in roles]) > limits.maximum_hypotheses: + raise ValueError("image_motion_hypothesis_limit") + training = _sample(list(range(0, len(frames), 2)), limits.maximum_training_frames) + validation = _sample([i for i in range(len(frames)) if i not in training], limits.maximum_validation_frames) + if min(len(training), len(validation)) < 12: + raise ValueError("image_motion_insufficient_independent_frames") + except (ValueError, TypeError) as error: + return ImageMotionResolution(False, str(error)) + hypotheses, models, residuals = [], [], [] + for indices in product(*(range(len(paths[r])) for r in roles)): + branches = tuple(zip(roles, indices)) + poses = {role: paths[role][index] for role, index in branches} + geometry = {"geometry_constraints": geometry_constraints, "source_urdf_sha256": source_urdf_sha256} + hypothesis, model, rms = _fit_hypothesis(frames, relations, poses, branches, training, validation, limits, + **(geometry if geometry_constraints else {})) + hypotheses.append(hypothesis) + models.append(model) + residuals.append(rms) + winner = training_candidate_index(hypotheses) + if winner is None or hypotheses[winner].reason: + return ImageMotionResolution(False, "image_motion_no_observable_image_model", hypotheses=tuple(hypotheses)) + model = models[winner] + checks = tuple(select_image_motion_frame(model, frames[i]) for i in validation) + if not all(c.resolved for c in checks): + return ImageMotionResolution(False, "image_motion_validation_frame_unresolved", hypotheses=tuple(hypotheses)) + angles = [[v for _, v in c.angles_rad] for c in checks] + winner_errors = np.array([np.sqrt(np.mean([q.rms_px ** 2 for q in c.quality])) for c in checks]) + comparisons = [] + for index, alternative in enumerate(models): + if index == winner: + continue + if alternative is None or residuals[index] is None or not hypotheses[index].converged: + # Missing candidate evidence is not evidence excluding a branch. + return ImageMotionResolution(False, "image_motion_alternative_not_evaluable", hypotheses=tuple(hypotheses)) + if hypotheses[index].reason == "image_motion_reprojection_failed": + # This converged model violates the unchanged absolute image gate + # on training/held-out images. It is already an inadmissible + # explanation, not a competing accepted family whose current-frame + # angle uncertainty should prevent the valid model being used. + continue + alternative_checks = tuple(select_image_motion_frame(alternative, frames[i]) for i in validation) + equivalent = all(equivalent_frame_selections(checked, other) + for checked, other in zip(checks, alternative_checks)) + if equivalent: + comparisons.append((index, 0.)) + continue + if not all(c.quality for c in alternative_checks): + return ImageMotionResolution(False, "image_motion_alternative_angles_unresolved", hypotheses=tuple(hypotheses)) + alternative_errors = np.array([np.sqrt(np.mean([q.rms_px ** 2 for q in c.quality])) + for c in alternative_checks]) + probability = _comparison(winner_errors, alternative_errors, angles, model.reprojection_tie_px) + comparisons.append((index, probability)) + adjusted = holm_adjusted_probabilities(comparisons) + hypotheses[winner] = replace(hypotheses[winner], comparison_p_values=tuple(comparisons), + comparison_adjusted_p_values=adjusted) + if any(probability > limits.familywise_error_probability for _, probability in adjusted): + return ImageMotionResolution(False, "image_motion_families_not_distinguishable", hypotheses=tuple(hypotheses)) + final = select_image_motion_frame(model, frames[-1]) + if not final.resolved: + return ImageMotionResolution(False, final.reason, hypotheses=tuple(hypotheses)) + return ImageMotionResolution(True, "", model, tuple(hypotheses), final.selections) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py index 238463c..c863b2b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py @@ -2,7 +2,10 @@ from __future__ import annotations -from dataclasses import replace +from contextlib import ExitStack +from copy import deepcopy +from dataclasses import asdict, dataclass, field, replace +from threading import RLock from typing import Sequence import math @@ -10,9 +13,69 @@ from scipy.spatial.transform import Rotation import cv2 import numpy as np -from .ippe import rotation_distance_rad, solve_square_tag_ippe -from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS, DEFAULT_REPROJECTION_TIE_PX +from .ippe import fixed_pose_reprojection_error, rotation_distance_rad, solve_square_tag_ippe +from .branch_confirmation import BranchConfirmation +from .fixed_reference import FixedTagReference +from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS, DEFAULT_REPROJECTION_TIE_PX, PoseTrackingParameters from .types import SquareTagPose +from .image_motion_commit import (VerifiedImageMotionDecision, + verify_image_motion_frame, commit_image_motion_frame) + + +@dataclass(frozen=True) +class TrackerImageObservation: + """Exact immutable pixels and optics retained independently of callers.""" + + stamp_ns: int + corners_xy: tuple[tuple[float, float], ...] + camera_matrix: tuple[tuple[float, float, float], ...] + tag_size_m: float + + +@dataclass(frozen=True) +class MotionTrackerSnapshot: + """One image's immutable inputs for a lock-free geometric decision.""" + + tracker: SquareTagPoseTracker = field(repr=False) + role: str + epoch: int + stamp_ns: int + revision: int + frozen: bool + candidates: tuple[SquareTagPose, ...] + previous_pose: SquareTagPose | None + evidence_id: str | None + motion_managed: bool + image_observation: TrackerImageObservation | None = None + + +@dataclass(frozen=True) +class MotionPoseSelection: + """A candidate selected by a previously checked geometric constraint.""" + + snapshot: MotionTrackerSnapshot + pose: SquareTagPose + evidence_id: str + + +@dataclass(frozen=True) +class MotionFrameCommit: + accepted: bool + reason: str + selections: tuple[MotionPoseSelection, ...] = () + observation_snapshots: tuple[MotionObservationSnapshot, ...] = () + + +@dataclass(frozen=True) +class MotionObservationSnapshot: + """Detached evidence captured in the same critical section as selection.""" + + tracker: SquareTagPoseTracker = field(repr=False) + role: str + stamp_ns: int + candidates: tuple[SquareTagPose, ...] + diagnostics: dict[str, object] + corrections: int def _validate_reprojection_thresholds(maximum_error: float, tie_error: float) -> None: @@ -42,13 +105,9 @@ def select_continuous_pose( maximum_rotation = float(maximum_pose_jump_rad) maximum_translation = float(maximum_translation_jump_m) maximum_tilt = float(maximum_tag_tilt_rad) - if min( - maximum_error, - maximum_rotation, - maximum_translation, - maximum_tilt, - ) <= 0.0: - raise ValueError("PnP selection thresholds must be positive") + if any(not math.isfinite(value) or value <= 0 for value in ( + maximum_error, maximum_rotation, maximum_translation, maximum_tilt)): + raise ValueError("PnP selection thresholds must be finite and positive") eligible: list[SquareTagPose] = [] for candidate in candidates: @@ -67,13 +126,9 @@ def select_continuous_pose( if previous is None: return best, "" - # Temporal continuity must only break a genuine planar-PnP tie. The old - # implementation normalised reprojection error by the permissive 1.5 px - # rejection limit, which allowed a stale mirror branch at 0.25 px to beat - # the true branch at e.g. 0.05 px merely because it was closer to the - # preceding (already wrong) pose. Once one IPPE solution has a meaningful - # image-fit advantage, trust it and allow the tracker to leave the stale - # branch even if that correction is a large pose jump. + # Image fit identifies a proposed branch. A stateless selector cannot + # confirm a large correction; the tracker below owns that evidence and + # knows whether a reference has already been frozen. competitive = [ candidate for candidate in eligible @@ -81,6 +136,14 @@ def select_continuous_pose( <= best.reprojection_error_px + tie_error ] if len(competitive) == 1: + nearest = min(eligible, key=lambda candidate: + rotation_distance_rad(previous.quaternion_xyzw, candidate.quaternion_xyzw) / maximum_rotation + + np.linalg.norm(np.asarray(previous.translation_xyz_m) - candidate.translation_xyz_m) / maximum_translation) + if (rotation_distance_rad(previous.quaternion_xyzw, best.quaternion_xyzw) > maximum_rotation + or np.linalg.norm(np.asarray(previous.translation_xyz_m) - best.translation_xyz_m) > maximum_translation + or rotation_distance_rad(nearest.quaternion_xyzw, best.quaternion_xyzw) + > DEFAULT_POSE_TRACKING_PARAMETERS.branch_equivalence_rad): + return None, "pose_branch_unconfirmed" return best, "" previous_translation = np.asarray(previous.translation_xyz_m, dtype=float) @@ -139,6 +202,9 @@ class SquareTagPoseTracker: maximum_translation_jump_m: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_translation_jump_m, maximum_tag_tilt_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad, reset_after_seconds: float = DEFAULT_POSE_TRACKING_PARAMETERS.reset_after_seconds, + branch_confirmation_frames: int = DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_frames, + branch_confirmation_minimum_seconds: float = DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_minimum_seconds, + branch_equivalence_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.branch_equivalence_rad, ) -> None: self.maximum_reprojection_error_px = float( maximum_reprojection_error_px @@ -150,23 +216,230 @@ class SquareTagPoseTracker: self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) self.maximum_translation_jump_m = float(maximum_translation_jump_m) self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad) + if any(not math.isfinite(v) or v <= 0 for v in ( + self.maximum_pose_jump_rad, self.maximum_translation_jump_m, + self.maximum_tag_tilt_rad, float(reset_after_seconds))): + raise ValueError("PnP tracking limits must be finite and positive") self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000) if self.reset_after_ns <= 0: raise ValueError("reset_after_seconds must be positive") - self._previous: dict[str, tuple[int, SquareTagPose]] = {} + self.branch_confirmation_frames = branch_confirmation_frames + self.branch_confirmation_minimum_seconds = float(branch_confirmation_minimum_seconds) + self.branch_equivalence_rad = float(branch_equivalence_rad) + self._lock = RLock() + self._epoch = 0 + self._branches = BranchConfirmation(PoseTrackingParameters( + self.maximum_reprojection_error_px, self.reprojection_tie_px, + self.maximum_pose_jump_rad, self.maximum_translation_jump_m, + self.maximum_tag_tilt_rad, float(reset_after_seconds), + branch_confirmation_frames, self.branch_confirmation_minimum_seconds, self.branch_equivalence_rad)) self.last_candidates_by_role: dict[ str, tuple[SquareTagPose, ...] ] = {} self.last_candidate_diagnostics_by_role: dict[ - str, dict[str, float | int] + str, dict[str, object] ] = {} - self.branch_correction_counts: dict[str, int] = {} + self.branch_correction_counts = self._branches.corrections + self._motion_candidates_by_role: dict[str, tuple[SquareTagPose, ...]] = {} + self._image_observations_by_role: dict[str, TrackerImageObservation] = {} + self._image_model_sha256_by_role: dict[str, str] = {} def reset(self) -> None: - self._previous.clear() - self.last_candidates_by_role.clear() - self.last_candidate_diagnostics_by_role.clear() - self.branch_correction_counts.clear() + with self._lock: + self._epoch += 1 + self._branches.reset() + self.last_candidates_by_role.clear() + self.last_candidate_diagnostics_by_role.clear() + self._motion_candidates_by_role.clear() + self._image_observations_by_role.clear() + self._image_model_sha256_by_role.clear() + + def observation_snapshot(self, role, stamp_ns): + """Copy one image's evidence atomically, never a subsequent callback.""" + with self._lock: + diagnostics = self.last_candidate_diagnostics_by_role.get(role, {}) + if diagnostics.get("observation_stamp_ns") != stamp_ns: + return None + return (self.last_candidates_by_role.get(role, ()), deepcopy(diagnostics), + self.branch_correction_counts.get(role, 0)) + + def motion_snapshot(self, role: str, stamp_ns: int) -> MotionTrackerSnapshot | None: + """Copy candidates and version metadata; callers solve outside locks.""" + with self._lock: + state = self._branches.states.get(role) + diagnostics = self.last_candidate_diagnostics_by_role.get(role, {}) + if state is None or diagnostics.get("observation_stamp_ns") != stamp_ns: + return None + return MotionTrackerSnapshot(self, role, self._epoch, stamp_ns, + state.revision, state.frozen, self._motion_candidates_by_role.get(role, ()), + state.pose, state.motion_evidence_id, diagnostics.get("motion_managed") is True, + self._image_observations_by_role.get(role)) + + @staticmethod + def _motion_snapshot_is_current(snapshot: MotionTrackerSnapshot) -> bool: + """Check metadata while the caller holds the corresponding tracker lock.""" + tracker = snapshot.tracker + state = tracker._branches.states.get(snapshot.role) + diagnostics = tracker.last_candidate_diagnostics_by_role.get(snapshot.role, {}) + return (tracker._epoch == snapshot.epoch and state is not None + and state.observed_ns == snapshot.stamp_ns and state.revision == snapshot.revision + and state.frozen == snapshot.frozen and state.motion_evidence_id == snapshot.evidence_id + and diagnostics.get("motion_managed") is True and snapshot.motion_managed + and diagnostics.get("motion_selection_committed") is not True) + + @staticmethod + def _motion_observation_snapshots(snapshots: Sequence[MotionTrackerSnapshot] + ) -> tuple[MotionObservationSnapshot, ...]: + """Detach diagnostics under the same locks used for a frame decision.""" + return tuple(MotionObservationSnapshot(snapshot.tracker, snapshot.role, snapshot.stamp_ns, + snapshot.tracker.last_candidates_by_role[snapshot.role], + deepcopy(snapshot.tracker.last_candidate_diagnostics_by_role[snapshot.role]), + snapshot.tracker.branch_correction_counts.get(snapshot.role, 0)) for snapshot in snapshots) + + @staticmethod + def reject_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], reason: str + ) -> MotionFrameCommit: + """Discard one unresolved image without changing a reference or model.""" + snapshots = tuple(snapshots) + if not snapshots or not isinstance(reason, str) or not reason: + return MotionFrameCommit(False, "pose_motion_selection_invalid") + keys = {(snapshot.tracker, snapshot.role) for snapshot in snapshots} + if len(keys) != len(snapshots): + return MotionFrameCommit(False, "pose_motion_selection_duplicate_role") + with ExitStack() as stack: + for tracker in sorted({tracker for tracker, _ in keys}, key=id): + stack.enter_context(tracker._lock) + if not all(SquareTagPoseTracker._motion_snapshot_is_current(snapshot) for snapshot in snapshots): + return MotionFrameCommit(False, "pose_observation_superseded") + result_reason = reason + for snapshot in snapshots: + tracker, role = snapshot.tracker, snapshot.role + state = tracker._branches.states[role] + state.pending = None + if state.conflict: + result_reason = "pose_branch_conflict" + tracker._branches.reject(state, result_reason, "frozen_conflict") + else: + tracker._branches.reject(state, reason, "motion_unavailable") + diagnostics = tracker.last_candidate_diagnostics_by_role[role] + diagnostics.update(tracker._branches.diagnostics(role)) + diagnostics["motion_selection_committed"] = True + diagnostics["pose_constraint"] = "motion_geometry" + return MotionFrameCommit(False, result_reason, observation_snapshots= + SquareTagPoseTracker._motion_observation_snapshots(snapshots)) + + @staticmethod + def commit_motion_frame(selections: Sequence[MotionPoseSelection], *, + establish: bool = False) -> MotionFrameCommit: + """Commit a joint geometric decision without interleaving image metadata. + + No candidate evaluation or model fitting happens here. Every candidate + must have passed this tracker's original image and tilt gates; every + snapshot must still identify the same image and reference revision. + Existing evidence identities, especially frozen ones, are immutable. + """ + selections = tuple(selections) + if not selections: + return MotionFrameCommit(False, "pose_motion_selection_missing") + keys = {(item.snapshot.tracker, item.snapshot.role) for item in selections} + if len(keys) != len(selections): + return MotionFrameCommit(False, "pose_motion_selection_duplicate_role") + with ExitStack() as stack: + for tracker in sorted({tracker for tracker, _ in keys}, key=id): + stack.enter_context(tracker._lock) + for item in selections: + snapshot = item.snapshot + tracker = snapshot.tracker + if not SquareTagPoseTracker._motion_snapshot_is_current(snapshot): + return MotionFrameCommit(False, "pose_observation_superseded") + if (not isinstance(item.evidence_id, str) or not item.evidence_id + or item.pose not in snapshot.candidates + or item.pose not in tracker._motion_candidates_by_role.get(snapshot.role, ())): + return MotionFrameCommit(False, "pose_motion_selection_invalid") + if any(item.snapshot.role in item.snapshot.tracker._image_model_sha256_by_role + for item in selections): + return MotionFrameCommit(False, "pose_motion_solver_changed") + return SquareTagPoseTracker._commit_motion_selections_locked(selections, establish=establish) + + @staticmethod + def _commit_motion_selections_locked(selections, *, establish, image_decision=None): + """Share the recovery/identity transaction across both trusted solvers.""" + trials = [] + for item in selections: + tracker, role = item.snapshot.tracker, item.snapshot.role + original = tracker._branches.states[role] + trial = replace(original, + pending=None if original.pending is None else replace(original.pending)) + pose, reason = tracker._branches.select_motion(trial, item.pose, + item.evidence_id, establish=establish) + trials.append((tracker, role, trial, pose, reason)) + failure = next((reason for _, _, _, pose, reason in trials if pose is None), "") + for tracker, role, trial, pose, _ in trials: + diagnostics = tracker.last_candidate_diagnostics_by_role[role] + diagnostics["motion_selection_committed"] = True + # Recovery needs several images, even if another role is not yet + # ready. A successful role cannot advance or establish a new + # identity when the joint decision as a whole was rejected. + if failure and pose is not None: + continue + original = tracker._branches.states[role] + if (not failure and original.motion_evidence_id is None and original.pose is not None + and not tracker._branches.continuous(original.pose, trial.pose)): + tracker.branch_correction_counts[role] = tracker.branch_correction_counts.get(role, 0) + 1 + tracker._branches.states[role] = trial + diagnostics.update(tracker._branches.diagnostics(role)) + diagnostics["pose_constraint"] = "motion_geometry" + if image_decision is not None and not failure: + diagnostics.update({ + "pose_constraint": "image_motion_geometry", + "pose_source": "image_constrained_current_image", + "frozen_image_model": asdict(image_decision.model), + "image_model_sha256": image_decision.model_sha256, + "image_motion_quality": [asdict(item) for item in image_decision.quality], + }) + tracker._image_model_sha256_by_role[role] = image_decision.model_sha256 + observations = SquareTagPoseTracker._motion_observation_snapshots( + tuple(item.snapshot for item in selections)) + if failure: + return MotionFrameCommit(False, failure, observation_snapshots=observations) + return MotionFrameCommit(True, "", selections, observations) + + verify_image_motion_frame = staticmethod(verify_image_motion_frame) + commit_image_motion_frame = staticmethod(commit_image_motion_frame) + + def freeze_roles(self, expected_revisions, *, require_motion_evidence=False): + return self.freeze_references([(self, role, revision) for role, revision in expected_revisions.items()], + require_motion_evidence=require_motion_evidence) + + @staticmethod + def freeze_references(references, *, require_motion_evidence=False): + """Atomically freeze metadata across views after evidence validation. + + Callers may hold the session lock. No PnP work takes place under these + locks, and tracker code never acquires the session lock in reverse. + """ + references = tuple(references) + if not references: + return False + roles = {(tracker, role) for tracker, role, _ in references} + required = (roles if require_motion_evidence is True else set() + if require_motion_evidence is False else set(require_motion_evidence)) + if not required.issubset(roles): + return False + with ExitStack() as stack: + for tracker in sorted({tracker for tracker, _, _ in references}, key=id): + stack.enter_context(tracker._lock) + for tracker, role, revision in references: + state = tracker._branches.states.get(role) + if (type(revision) is not int or state is None or state.revision != revision or state.status != "tracking" + or state.pending is not None or state.conflict or state.recovery_required): + return False + if (tracker, role) in required and state.motion_evidence_id is None: + return False + for tracker, role, _ in references: + tracker._branches.states[role].frozen = True + tracker.last_candidate_diagnostics_by_role[role]["branch_frozen"] = True + return True def estimate( self, @@ -177,7 +450,15 @@ class SquareTagPoseTracker: camera_matrix: Sequence[Sequence[float]], stamp_ns: int, reprojection_tie_px: float | None = None, + fixed_reference: FixedTagReference | None = None, + motion_managed: bool = False, ) -> tuple[SquareTagPose | None, str]: + with self._lock: + epoch = self._epoch + image_observation = TrackerImageObservation(int(stamp_ns), + tuple(tuple(float(v) for v in point) for point in corners_xy), + tuple(tuple(float(v) for v in row) for row in camera_matrix), float(tag_size_m)) + corners_xy, camera_matrix = image_observation.corners_xy, image_observation.camera_matrix try: candidates = solve_square_tag_ippe( corners_xy, @@ -185,33 +466,7 @@ class SquareTagPoseTracker: camera_matrix=camera_matrix, ) except (ValueError, cv2.error): - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" - if not candidates: - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" + candidates = [] reprojection_candidates = [ candidate for candidate in candidates @@ -242,19 +497,12 @@ class SquareTagPoseTracker: # image detections. Group tracking therefore receives every # reprojection-valid candidate; independent tracking below retains the # original tilt safety gate. - self.last_candidates_by_role[str(role)] = tuple( - reprojection_candidates - ) - diagnostics: dict[str, float | int] = { + diagnostics: dict[str, object] = { + "motion_managed": bool(motion_managed and fixed_reference is None), + "motion_selection_committed": False, "solved_candidate_count": len(candidates), "reprojection_candidate_count": len(reprojection_candidates), "independent_tilt_candidate_count": len(independent_candidates), - "minimum_reprojection_error_px": float( - min( - candidate.reprojection_error_px - for candidate in candidates - ) - ), "maximum_reprojection_error_px": float( self.maximum_reprojection_error_px ), @@ -262,6 +510,8 @@ class SquareTagPoseTracker: self.maximum_tag_tilt_rad ), } + if candidates: + diagnostics["minimum_reprojection_error_px"] = min(c.reprojection_error_px for c in candidates) if candidate_tilts_rad: diagnostics["minimum_candidate_tilt_deg"] = math.degrees( min(candidate_tilts_rad) @@ -269,56 +519,44 @@ class SquareTagPoseTracker: diagnostics["maximum_candidate_tilt_deg"] = math.degrees( max(candidate_tilts_rad) ) - self.last_candidate_diagnostics_by_role[str(role)] = diagnostics - if not independent_candidates: - return None, "no_pose_within_reprojection_or_tilt_limit" - - previous_record = self._previous.get(str(role)) - previous: SquareTagPose | None = None - if previous_record is not None: - previous_stamp, previous_pose = previous_record - elapsed = int(stamp_ns) - previous_stamp - if 0 <= elapsed <= self.reset_after_ns: - previous = previous_pose - - selected, reason = select_continuous_pose( - independent_candidates, - previous=previous, - maximum_reprojection_error_px=( - self.maximum_reprojection_error_px - ), - reprojection_tie_px=( - self.reprojection_tie_px - if reprojection_tie_px is None - else float(reprojection_tie_px) - ), - maximum_pose_jump_rad=self.maximum_pose_jump_rad, - maximum_translation_jump_m=self.maximum_translation_jump_m, - maximum_tag_tilt_rad=self.maximum_tag_tilt_rad, - ) - if selected is not None: - if previous is not None: - rotation_jump = rotation_distance_rad( - previous.quaternion_xyzw, - selected.quaternion_xyzw, - ) - translation_jump = float( - np.linalg.norm( - np.asarray(selected.translation_xyz_m, dtype=float) - - np.asarray( - previous.translation_xyz_m, - dtype=float, - ) - ) - ) - if ( - rotation_jump > self.maximum_pose_jump_rad - or translation_jump - > self.maximum_translation_jump_m - ): - key = str(role) - self.branch_correction_counts[key] = ( - self.branch_correction_counts.get(key, 0) + 1 - ) - self._previous[str(role)] = (int(stamp_ns), selected) + tie = self.reprojection_tie_px if reprojection_tie_px is None else float(reprojection_tie_px) + _validate_reprojection_thresholds(self.maximum_reprojection_error_px, tie) + checked_fixed_pose = None + if fixed_reference is not None: + # Projection is outside the metadata lock, just like IPPE. Keep + # all current candidates as evidence, even when their ranking + # changes while the frozen fixed pose still explains the image. + diagnostics["pose_constraint"] = "fixed_reference" + try: + error = fixed_pose_reprojection_error(fixed_reference.pose, corners_xy, + tag_size_m=tag_size_m, camera_matrix=camera_matrix) + normal = Rotation.from_quat(fixed_reference.pose.quaternion_xyzw).as_matrix()[:, 2] + tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0))) + diagnostics["fixed_reference_reprojection_error_px"] = error + if (error <= self.maximum_reprojection_error_px + and tilt <= self.maximum_tag_tilt_rad and independent_candidates): + checked_fixed_pose = replace(fixed_reference.pose, reprojection_error_px=error) + except (ValueError, cv2.error): + pass + diagnostics["fixed_reference_verified"] = checked_fixed_pose is not None + with self._lock: + if epoch != self._epoch: + return None, "pose_observation_superseded" + if fixed_reference is None and motion_managed: + selected, reason = self._branches.observe_motion(str(role), independent_candidates, int(stamp_ns)) + elif fixed_reference is None: + selected, reason = self._branches.update(str(role), independent_candidates, int(stamp_ns), tie) + else: + selected, reason = self._branches.update_fixed(str(role), fixed_reference, + checked_fixed_pose, int(stamp_ns)) + if reason == "pose_observation_not_new": + return None, reason + if not candidates and reason == "no_pose_within_reprojection_or_tilt_limit": + reason = "pnp_solve_failed" + self._branches.states[str(role)].reason = reason + diagnostics.update(self._branches.diagnostics(str(role))) + self.last_candidates_by_role[str(role)] = tuple(reprojection_candidates) + self._motion_candidates_by_role[str(role)] = tuple(independent_candidates) + self._image_observations_by_role[str(role)] = image_observation + self.last_candidate_diagnostics_by_role[str(role)] = diagnostics return selected, reason diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py index 10cb4ca..022e310 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py @@ -16,6 +16,11 @@ from .patch import validate_urdf_mimic_ranges from .validate import validate_runtime_curve_limits, validate_structural_urdf_diff +ANGLE_MAE_LIMIT_DEG = 1.0 +ANGLE_P95_LIMIT_DEG = 2.0 +ANGLE_MAXIMUM_LIMIT_DEG = 3.0 + + @dataclass(frozen=True) class JointHoldout: """Independent observation in the corrected coordinate convention. @@ -45,7 +50,9 @@ class ErrorMetrics: @property def passed(self) -> bool: - return self.count > 0 and self.mae_deg <= 1.0 and self.p95_deg <= 2.0 and self.maximum_deg <= 3.0 + return (self.count > 0 and self.mae_deg <= ANGLE_MAE_LIMIT_DEG + and self.p95_deg <= ANGLE_P95_LIMIT_DEG + and self.maximum_deg <= ANGLE_MAXIMUM_LIMIT_DEG) def angular_metrics(errors_rad: Sequence[float]) -> ErrorMetrics: @@ -59,8 +66,9 @@ def angular_metrics(errors_rad: Sequence[float]) -> ErrorMetrics: class SerializedJointMapping: """Read the numeric legacy v4/v6/v7 curve shapes without model imports. - Byte payloads preserve their deployed nearest-integer lookup; continuous - payloads interpolate explicit knots. Neither path silently clamps inputs. + Legacy byte tables and byte commands retain nearest-integer lookup. + Unified feedback uses its declared piecewise-linear knots, including + feedback interpolated to image time. Neither path silently clamps inputs. """ def __init__(self, payload: Mapping[str, Any], *, input_kind=None) -> None: @@ -68,6 +76,14 @@ class SerializedJointMapping: self.measured = payload.get("format") in {"unified_calibration_v3", "unified_calibration_report_v3"} self.compact = payload.get("format") in {"unified_calibration_v2", "unified_calibration_v3"} input_kind = input_kind or ("command" if self.compact else "feedback") + declared_directions = payload.get("directional_command_joints", []) + if (not isinstance(declared_directions, list) or any(not isinstance(n, str) for n in declared_directions) + or len(set(declared_directions)) != len(declared_directions) + or not set(declared_directions) <= payload.get("joints", {}).keys() + or declared_directions and not self.measured): + raise ValueError("invalid directional command declaration") + self.directional_joints = frozenset(declared_directions) if input_kind == "command" else frozenset() + self.baseline = payload.get("baseline_command", ()) self.unified = payload.get("format") in {"unified_calibration_v1", "unified_calibration_report_v3"} if self.compact: if input_kind != "command" or payload.get("schema_version") != (3 if self.measured else 2): @@ -78,11 +94,15 @@ class SerializedJointMapping: joints = {} for name, row in payload.get("joints", {}).items(): expected = {"sdk_channel", "angle_rad"} | ({"input_values"} if unit == "rad" else set()) + if name in self.directional_joints: + expected |= {"increasing_rad", "decreasing_rad"} index = row.get("sdk_channel") if set(row) != expected or type(index) is not int or index < 0: raise ValueError(f"invalid compact joint/channel fields:{name}") joints[name] = {"motor_index": index, "angle_rad": row["angle_rad"], "curve_input_knots_rad": row.get("input_values", ())} + if name in self.directional_joints: + joints[name].update({key: row[key] for key in ("increasing_rad", "decreasing_rad")}) payload = {"joints": joints, "curve_input_domain": f"command_{unit}"} elif self.unified: if input_kind not in {"command", "feedback"} or payload.get("schema_version") != (3 if self.measured else 1): @@ -153,6 +173,9 @@ class SerializedJointMapping: selected = direction if directions_by_joint is None else directions_by_joint.get(name, "") if selected not in {"increasing", "decreasing", ""}: raise ValueError("invalid per-joint SDK direction") + if name in self.directional_joints and not selected: + if index >= len(self.baseline) or value != self.baseline[index]: + raise ValueError(f"directional_command_requires_direction:{name}") if self.domain.endswith("_rad") and selected: increasing_branch = str(row.get("raw_increasing_curve_branch", "increasing")) if increasing_branch not in {"increasing", "decreasing"}: @@ -165,10 +188,10 @@ class SerializedJointMapping: angle = float(values[int(math.floor(value + 0.5))]) else: knots = row["curve_input_knots_rad"] - if self.domain.endswith("_u8"): - value = float(math.floor(value+0.5)) if not knots[0] <= value <= knots[-1]: raise ValueError(f"holdout input is outside serialized knot domain: {name}") + if self.domain == "command_u8": + value = float(math.floor(value+0.5)) angle = float(np.interp(value, knots, values)) output[name] = angle return output @@ -246,15 +269,41 @@ def validate_standard_urdf_holdout( def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str | Path, source_sha256: str, authorized_fields: Mapping[str, Sequence[str]], - payload: Mapping[str, Any], zero_offsets_rad: Mapping[str, float]) -> tuple[str, ...]: - """Validate physical ranges in CAD coordinates, including coordinate shifts.""" + payload: Mapping[str, Any], zero_offsets_rad: Mapping[str, float], + range_evidence=None) -> tuple[str, ...]: + """Validate source/safety bounds or exact trusted training motion ranges.""" if len(source_sha256) != 64 or hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() != source_sha256: raise ValueError("protected source URDF SHA256 mismatch") + before, after = UrdfKinematicModel(source_urdf), UrdfKinematicModel(corrected_urdf) + from .joint_control import independent_joint_sources, independent_export_authorization, validate_independent_output, uses_source_mimic + mapping = SerializedJointMapping(payload) + sources = independent_joint_sources(payload) + if sources is not None: + channels = {name: row["motor_index"] for name, row in mapping.joints.items()} + authorized_fields = independent_export_authorization(before, channels, sources, authorized_fields) + validate_independent_output(after, channels, sources) changes = validate_structural_urdf_diff(source_urdf, corrected_urdf, authorized_fields=authorized_fields, expected_source_sha256=source_sha256) - before, after = UrdfKinematicModel(source_urdf), UrdfKinematicModel(corrected_urdf) + from .limits import validate_range_evidence + motion_ranges = dict(range_evidence or {}) + if not set(motion_ranges) <= before.joints.keys(): + raise ValueError("range evidence references unknown joints") if not set(zero_offsets_rad) <= before.joints.keys(): raise ValueError("declared coordinate zero targets missing joints") + envelopes = {} + if uses_source_mimic(payload): + from .source_mimic import compile_source_mimic_envelopes, validate_source_mimic_elements, validate_serialized_mimic_envelopes + validate_source_mimic_elements(source_urdf, corrected_urdf) + envelopes = compile_source_mimic_envelopes(before, + {name: (j.lower, j.upper) for name, j in after.joints.items() if j.lower is not None}, + zero_offsets_rad, motion_ranges) + validate_serialized_mimic_envelopes(payload, after) + from dataclasses import asdict + for name, envelope in envelopes.items(): + declared = payload["urdf_correction"]["mimic_envelopes"][name] + if any(not np.allclose(values, declared[key], atol=1e-10, rtol=0) + for key, values in asdict(envelope).items()): + raise ValueError(f"source_mimic_envelope_differs_from_trusted_ranges:{name}") # No model bypass, no inherited-CAD excess allowance at final acceptance. validate_urdf_mimic_ranges(corrected_urdf) for name, joint in after.joints.items(): @@ -263,12 +312,14 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str if not math.isfinite(delta): raise ValueError("nonfinite coordinate zero") if joint.lower is not None and original.lower is not None: - if joint.lower + delta < original.lower - 1e-8 or joint.upper + delta > original.upper + 1e-8: + if name in motion_ranges: + interval = envelopes[name].base_limits_rad if name in envelopes else (joint.lower, joint.upper) + validate_range_evidence(name, original, delta, interval, motion_ranges[name]) + elif joint.lower + delta < original.lower - 1e-8 or joint.upper + delta > original.upper + 1e-8: raise ValueError(f"corrected range exceeds source mechanical range: {name}") expected = original.origin @ before._motion(original, delta) if not np.allclose(expected, joint.origin, atol=1e-8, rtol=0): raise ValueError(f"URDF origin does not match declared coordinate zero: {name}") - mapping = SerializedJointMapping(payload) if mapping.compact: validate_compact_urdf_tables(payload, after) for branch in ("angle_rad", "increasing_rad", "decreasing_rad"): @@ -280,6 +331,13 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str validate_runtime_curve_limits(corrected_urdf, {name: row[branch] for name, row in commands.joints.items()}) for name, row in payload["joints"].items(): + if sources is not None: + if row.get("passive") != (name in sources): + raise ValueError(f"passive_provenance_differs_from_source_urdf:{name}") + if name in sources and (row.get("mimic_approximation", {}).get("joint") != sources[name] + or row["mimic_approximation"].get("usage") != "diagnostic_only_not_exported"): + raise ValueError(f"diagnostic_mimic_source_differs:{name}") + continue if row.get("passive"): joint = after.joints[name] expected = {"joint": joint.mimic_joint, "multiplier": joint.mimic_multiplier, @@ -293,20 +351,31 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str def validate_compact_urdf_tables(payload, model): - """Redundant passive lookup entries must equal the written URDF, recursively.""" + """Validate curve ranges and the declared URDF angle-provider contract.""" mapping = SerializedJointMapping(payload, input_kind="command") + from .joint_control import independent_joint_sources, validate_independent_output, uses_affine_mimic_approximation, uses_source_mimic + sources = independent_joint_sources(payload) + preserved = uses_source_mimic(payload) + affine = uses_affine_mimic_approximation(payload) or preserved + if preserved: + from .source_mimic import validate_serialized_mimic_envelopes + validate_serialized_mimic_envelopes(payload, model) + if sources is not None: + validate_independent_output(model, {name: row["motor_index"] for name, row in mapping.joints.items()}, sources) moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"} if set(mapping.joints) != moving: raise ValueError("compact JSON joints differ from the exported URDF") for name, row in mapping.joints.items(): joint = model.joints[name] values = np.asarray(row["angle_rad"]) - if joint.lower is not None and (np.min(values) < joint.lower-1e-9 or np.max(values) > joint.upper+1e-9): - raise ValueError(f"compact table exceeds exported URDF range:{name}") + for branch in ("angle_rad", "increasing_rad", "decreasing_rad"): + branch_values = np.asarray(row.get(branch, row["angle_rad"])) + if joint.lower is not None and (np.min(branch_values) < joint.lower-1e-9 or np.max(branch_values) > joint.upper+1e-9): + raise ValueError(f"compact table exceeds exported URDF range:{name}:{branch}") if joint.mimic_joint: parent = mapping.joints[joint.mimic_joint] if mapping.measured: - if row["motor_index"] != parent["motor_index"] or abs(joint.mimic_offset) > 1e-10: + if row["motor_index"] != parent["motor_index"] or (not affine and abs(joint.mimic_offset) > 1e-10): raise ValueError(f"v3 mimic channel or shared zero differs:{name}") continue if (row["motor_index"] != parent["motor_index"] @@ -323,6 +392,8 @@ def validate_compact_urdf_tables(payload, model): index = row["motor_index"] if index >= len(baseline): raise ValueError("v3 baseline channel missing") - value = mapping.evaluate(baseline, "", active_joints=(name,))[name] - if abs(value) > 1e-10: - raise ValueError(f"v3 baseline is not zero:{name}") + branches = ("", "increasing", "decreasing") if name in mapping.directional_joints else ("",) + for direction in branches: + value = mapping.evaluate(baseline, direction, active_joints=(name,))[name] + if abs(value) > 1e-10: + raise ValueError(f"v3 baseline is not zero:{name}:{direction}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py new file mode 100644 index 0000000..4f4fbc7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py @@ -0,0 +1,165 @@ +"""Validate frozen final-file motion and its independent image evidence. + +Primary measured poses must explain their original pixels. Their difference +from final-file motion is judged in degrees and metres, not by reusing the PnP +pixel limit for the sum of measurement and motion error. Extra views provide a +separate consistency check, never a substitute for independent 3-D accuracy. +No shared calibration parameter is estimated or changed during acceptance. +""" + +from dataclasses import asdict, dataclass + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..fitting.tag_installation import rigid_matrix +from ..geometry.tag_pose.ippe import square_object_points +from ..geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS +from .acceptance import SerializedJointMapping, angular_metrics +from .image_consistency import find_projection_witness, projection_error +from .kinematics import UrdfKinematicModel +from .joint_control import independent_joint_sources +from .tag_acceptance import POSITION_P95_LIMIT_M + + +FINAL_IMAGE_ACCEPTANCE_POLICY = "independent_measurement_and_physical_consistency_v2" + + +@dataclass(frozen=True) +class TagImageObservation: + sample_id: str + role: str + cycle: int + sdk_values: tuple[float, ...] + directions: tuple[tuple[str, str], ...] + view: str + tag_size_m: float + corners_xy: tuple[tuple[float, float], ...] + camera_matrix: tuple[tuple[float, float, float], ...] + view_from_common: tuple[tuple[float, ...], ...] + task_name: str = "" + + +class ImageHoldoutFailure(ValueError): + def __init__(self, report, *, input_kind, holdout_cycle): + self.report = report + self.input_kind = input_kind + self.holdout_cycle = holdout_cycle + failed = [role for role, metrics in report.items() if not metrics["passed"]] + super().__init__("final_image_holdout_failed:" + input_kind + ":" + ",".join(failed)) + + +def _physical_metrics(values): + angles = angular_metrics([angle for angle, _ in values]) + position = float(np.percentile([distance for _, distance in values], 95)) + return {**asdict(angles), "translation_p95_m": position, + "passed": angles.passed and position <= POSITION_P95_LIMIT_M} + + +def _pixel_metrics(values): + return {"rms_px": float(np.sqrt(np.mean(np.square(values)))), + "p95_px": float(np.percentile(values, 95)), "maximum_px": max(values)} + + +def _image_geometry(row): + if not np.isfinite(row.tag_size_m) or row.tag_size_m <= 0: + raise ValueError("final_image_holdout_size_invalid") + matrix, measured = np.asarray(row.camera_matrix), np.asarray(row.corners_xy) + if (matrix.shape != (3, 3) or measured.shape != (4, 2) + or not np.all(np.isfinite(matrix)) or not np.all(np.isfinite(measured)) + or matrix[0, 0] <= 0 or matrix[1, 1] <= 0 or not np.array_equal(matrix[2], [0, 0, 1])): + raise ValueError("final_image_holdout_projection_invalid") + return matrix, measured, square_object_points(row.tag_size_m) + + +def validate_serialized_image_holdout(*, corrected_urdf, payload, common_from_base, + installations, observations, independent_observations, required_roles, input_kind, + holdout_cycle, minimum_samples): + """Keep 1.5 px measurement accuracy and the original 3-D motion limits. + + Auxiliary witnesses only prove image compatibility within those limits. + They are grouped by motion task so stationary images cannot dilute the + errors in a different moving task. Independent poses remain mandatory. + """ + roles = frozenset(required_roles) + if (holdout_cycle not in {3, 5} or not roles or not roles <= installations.keys() + or minimum_samples < 1): + raise ValueError("final_image_holdout_contract_invalid") + model = UrdfKinematicModel(corrected_urdf) + mapping = SerializedJointMapping(payload, input_kind=input_kind) + active = tuple(name for name, joint in model.joints.items() + if joint.kind != "fixed" and (mapping.measured or joint.mimic_joint is None)) + base = rigid_matrix(common_from_base) + training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids} + independent = {} + for pose in independent_observations: + identity = (pose.role, pose.sample_id) + if (identity in independent or pose.role not in roles or not pose.sample_id + or pose.cycle != holdout_cycle or pose.sample_id in training_ids): + raise ValueError("final_image_holdout_independent_identity_invalid") + independent[identity] = pose + errors = {role: [] for role in roles} + motion = {role: [] for role in roles} + auxiliary = {role: {} for role in roles} + seen = set() + limit = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px + for row in observations: + identity = (row.role, row.sample_id) + if (row.cycle != holdout_cycle or row.sample_id in training_ids or not row.sample_id + or identity in seen or row.role not in roles): + raise ValueError("final_image_holdout_identity_invalid") + seen.add(identity) + mount = installations[row.role] + if mount.role != row.role or mount.link not in model.links: + raise ValueError("final_image_holdout_installation_invalid") + angles = mapping.evaluate(row.sdk_values, "", active_joints=active, + directions_by_joint=dict(row.directions)) + link = np.eye(4) if mount.link == model.root_link else model.link_transform( + model.parent_joint_by_child[mount.link], zero_offsets={}, joint_angles=angles, + independent_mimic_angles=mapping.measured and independent_joint_sources(payload) is None) + predicted = rigid_matrix(row.view_from_common) @ base @ link @ rigid_matrix(mount.link_from_tag) + camera, pixels, objects = _image_geometry(row) + nominal = projection_error(predicted, camera, objects, pixels) + measured = independent.get(identity) + if measured is not None: + if (tuple(measured.sdk_values) != row.sdk_values + or dict(measured.sdk_directions_by_joint) != dict(row.directions)): + raise ValueError("final_image_holdout_independent_input_changed") + observed = rigid_matrix(row.view_from_common) @ rigid_matrix(measured.common_from_tag) + residual = projection_error(observed, camera, objects, pixels) + angle = float(Rotation.from_matrix(predicted[:3, :3].T @ observed[:3, :3]).magnitude()) + distance = float(np.linalg.norm(predicted[:3, 3]-observed[:3, 3])) + motion[row.role].append((angle, distance)) + else: + if not row.task_name: + raise ValueError("final_image_holdout_auxiliary_task_missing") + witness = find_projection_witness(predicted, camera, objects, pixels, image_limit_px=limit) + residual = witness.residual_px + auxiliary[row.role].setdefault(row.task_name, []).append(witness) + errors[row.role].append((row, nominal, residual)) + results = {} + for role in sorted(roles): + if len(motion[role]) < minimum_samples: + raise ValueError(f"final_image_holdout_missing:{role}:{len(motion[role])}") + values = errors[role] + residuals = [value[2] for value in values] + physical = _physical_metrics(motion[role]) + tasks = {} + for task, witnesses in auxiliary[role].items(): + metrics = _physical_metrics([(row.angle_rad, row.position_m) for row in witnesses]) + tasks[task] = {**metrics, "interpretation": "feasible_pose_displacement_only", + "optimizer_not_converged_count": sum(not row.optimizer_converged for row in witnesses), + "maximum_px": max(row.residual_px for row in witnesses)} + tasks[task]["passed"] = metrics["passed"] and tasks[task]["maximum_px"] <= limit + worst = values[int(np.argmax(residuals))][0] + results[role] = {"count": len(values), **_pixel_metrics(residuals), "limit_px": limit, + "nominal_projection": _pixel_metrics([value[1] for value in values]), + "independent_motion": physical, "auxiliary_consistency_by_task": tasks, + "worst_sample_id": worst.sample_id, "view": worst.view, + "passed": (max(residuals) <= limit and physical["passed"] + and all(value["passed"] for value in tasks.values()))} + if not independent.keys() <= seen: + raise ValueError("final_image_holdout_independent_source_missing") + if not all(metrics["passed"] for metrics in results.values()): + raise ImageHoldoutFailure(results, input_kind=input_kind, holdout_cycle=holdout_cycle) + return results diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_consistency.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_consistency.py new file mode 100644 index 0000000..b0b0fd3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_consistency.py @@ -0,0 +1,109 @@ +"""Check whether extra-view pixels admit a pose within a physical error budget. + +A local pose is an existence witness, not an independently measured pose. It +never changes the base, mounting, camera, joints or exported calibration. The +caller must separately validate independent 3-D observations and check the +witness displacement against the same physical accuracy limits. +""" + +from dataclasses import dataclass + +import numpy as np +from scipy.optimize import minimize +from scipy.spatial.transform import Rotation + +from .acceptance import ANGLE_MAXIMUM_LIMIT_DEG +from .tag_acceptance import POSITION_P95_LIMIT_M + + +@dataclass(frozen=True) +class ProjectionConsistency: + nominal_px: float + residual_px: float + angle_rad: float + position_m: float + iterations: int + optimizer_converged: bool + + +def _cross_matrix(vector): + a, b, c = vector + return np.array([[0., -c, b], [c, 0., -a], [-b, a, 0.]]) + + +def _rigid_projection(x, pose, camera, objects, measured, angle_scale, position_scale): + """Pixel residual and analytic Jacobian for a Tag-centred rigid displacement.""" + vector = x[:3] * angle_scale + theta = float(np.linalg.norm(vector)) + skew = _cross_matrix(vector) + if theta < 1e-4: + left = np.eye(3) + (.5-theta**2/24)*skew + (1/6-theta**2/120)*(skew @ skew) + else: + left = (np.eye(3) + (1-np.cos(theta))/theta**2*skew + + (theta-np.sin(theta))/theta**3*(skew @ skew)) + rotated = objects @ (Rotation.from_rotvec(vector).as_matrix() @ pose[:3, :3]).T + points = rotated + pose[:3, 3] + x[3:] * position_scale + if not np.all(np.isfinite(points)) or np.any(points[:, 2] <= 0): + raise ValueError("final_image_holdout_projection_invalid") + homogeneous = points @ camera.T + pixels = homogeneous[:, :2] / homogeneous[:, 2:] + point_jacobian = np.empty((4, 3, 6)) + point_jacobian[:, :, :3] = [-_cross_matrix(point) @ left * angle_scale for point in rotated] + point_jacobian[:, :, 3:] = np.eye(3) * position_scale + projection = ((camera[:2, :][None] - pixels[:, :, None]*camera[2, :][None, None]) + / homogeneous[:, 2, None, None]) + return pixels-measured, np.einsum("nki,nij->nkj", projection, point_jacobian) + + +def projection_error(pose, camera, objects, measured): + residual, _ = _rigid_projection(np.zeros(6), pose, camera, objects, measured, 1., 1.) + return float(np.sqrt(np.mean(np.sum(residual**2, axis=1)))) + + +def find_projection_witness(pose, camera, objects, measured, *, image_limit_px): + """Find a feasible pose; optimizer success alone never establishes acceptance. + + The objective favours a small displacement in physical units. Its returned + displacement is not claimed to be globally minimal. The caller checks both + the directly evaluated pixel residual and the physical displacement. + """ + angle_scale = np.deg2rad(ANGLE_MAXIMUM_LIMIT_DEG) + position_scale = POSITION_P95_LIMIT_M + + def evaluate(x): + return _rigid_projection(x, pose, camera, objects, measured, angle_scale, position_scale) + + def rms(x): + residual, _ = evaluate(x) + return float(np.sqrt(np.mean(np.sum(residual**2, axis=1)))) + + zero = np.zeros(6) + nominal = rms(zero) + if nominal <= image_limit_px: + return ProjectionConsistency(nominal, nominal, 0., 0., 0, True) + # A small inward margin avoids accepting a constraint rounded above its + # limit. It does not enlarge either image or physical tolerances. + target = image_limit_px * (1.-1e-6) + + def constraint_jacobian(x): + residual, jacobian = evaluate(x) + return -2*np.mean(np.einsum("nk,nkj->nj", residual, jacobian), axis=0) + + constraint = {"type": "ineq", "fun": lambda x: target**2-rms(x)**2, + "jac": constraint_jacobian} + # Bounds limit numerical exploration, not the permitted physical error. + # Ensure every explored point remains in front of the image plane. + depth = pose[2, 3] - float(np.max(np.linalg.norm(objects, axis=1))) + lower_z = max(-5., -depth / position_scale * (1.-1e-6)) + if lower_z >= 0: + raise ValueError("final_image_holdout_projection_invalid") + bounds = [(-5., 5.)]*5 + [(lower_z, 5.)] + result = minimize(lambda x: float(x @ x), zero, jac=lambda x: 2*x, + constraints=constraint, bounds=bounds, method="SLSQP", + options={"maxiter": 60, "ftol": 1e-10}) + if not np.all(np.isfinite(result.x)): + raise ValueError("final_image_holdout_witness_nonfinite") + return ProjectionConsistency(nominal, rms(result.x), + float(np.linalg.norm(result.x[:3])*angle_scale), + float(np.linalg.norm(result.x[3:])*position_scale), + int(result.nit), bool(result.success)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/joint_control.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/joint_control.py new file mode 100644 index 0000000..8951e8e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/joint_control.py @@ -0,0 +1,78 @@ +"""Versioned roles of measured JSON and standard URDF mimic approximations. + +Production keeps mimic for standalone linear motion and uses explicit JSON +angles for measured motion. Version 2's mimic-free export remains readable; +its removal rights never authorize extra geometry or SDK actuation changes. +""" + +JSON_JOINT_ANGLES = "calibration_json" +AFFINE_MIMIC_APPROXIMATION = "range_bounded_affine_approximation" +PRESERVE_SOURCE_MIMIC = "preserve_source_mimic" + + +def uses_source_mimic(payload): + correction = payload.get("urdf_correction", {}) + if correction.get("schema_version") != 4: + return False + if (payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"} + or correction.get("joint_angle_source") != JSON_JOINT_ANGLES + or correction.get("mimic_policy") != PRESERVE_SOURCE_MIMIC): + raise ValueError("unsupported_source_mimic_contract") + return True + + +def uses_affine_mimic_approximation(payload): + correction = payload.get("urdf_correction", {}) + if correction.get("schema_version") != 3: + return False + if (payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"} + or correction.get("joint_angle_source") != JSON_JOINT_ANGLES + or correction.get("mimic_policy") != AFFINE_MIMIC_APPROXIMATION): + raise ValueError("unsupported_urdf_mimic_approximation_contract") + return True + + +def independent_joint_sources(payload): + """None denotes a URDF retaining its linear mimic constraints.""" + correction = payload.get("urdf_correction", {}) + version = correction.get("schema_version", 1) + if version == 1 or uses_affine_mimic_approximation(payload) or uses_source_mimic(payload): + return None + if (version != 2 + or payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"} + or correction.get("joint_angle_source") != JSON_JOINT_ANGLES): + raise ValueError("unsupported_urdf_joint_control_contract") + sources = correction.get("passive_joint_sources") + if not isinstance(sources, dict) or any( + not isinstance(name, str) or not isinstance(parent, str) or name == parent + for name, parent in sources.items() + ): + raise ValueError("invalid_independent_passive_joint_sources") + return sources + + +def validate_joint_channels(model, channels, sources): + moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"} + if set(channels) != moving or any(type(index) is not int or index < 0 for index in channels.values()): + raise ValueError("independent_joint_channel_coverage_differs") + for name, parent in sources.items(): + if name not in moving or parent not in moving or channels[name] != channels[parent]: + raise ValueError(f"independent_passive_sdk_channel_differs:{name}") + + +def independent_export_authorization(model, channels, sources, authorized_fields): + """Authorize only the exact source mimic removals, after binding validation.""" + original = {name: joint.mimic_joint for name, joint in model.joints.items() if joint.mimic_joint} + if sources != original: + raise ValueError("independent_passive_sources_differ_from_source_urdf") + validate_joint_channels(model, channels, sources) + rights = {name: tuple(fields) for name, fields in authorized_fields.items()} + for name in sources: + rights[name] = tuple(sorted(set(rights.get(name, ())) | {"mimic.remove"})) + return rights + + +def validate_independent_output(model, channels, sources): + validate_joint_channels(model, channels, sources) + if any(joint.mimic_joint is not None for joint in model.joints.values()): + raise ValueError("json_joint_angles_must_not_be_overridden_by_urdf_mimic") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py new file mode 100644 index 0000000..a7402ad --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py @@ -0,0 +1,102 @@ +"""Training-supported URDF motion envelopes and independent safety bounds. + +CAD limits remain hard bounds unless the protected Profile explicitly declares +them to be nominal motion envelopes. This module never changes SDK commands, +adds tolerance padding, fits a curve, or consumes validation observations. +""" + +from dataclasses import dataclass +import math + + +@dataclass(frozen=True) +class MotionRangeEvidence: + joint: str + source_joint: str + independently_measured: bool + source_limits_cad_rad: tuple[float, float] + output_limits_rad: tuple[float, float] + feedback_bounds_rad: tuple[float, float] + command_bounds_rad: tuple[float, float] + zero_offset_rad: float + baseline_command: float + training_cycles: tuple[int, ...] + training_sample_ids: tuple[str, ...] + reference_sample_ids: tuple[str, ...] + source_limits: str + evidence_source: str + independent_safety_limits_cad_rad: tuple[float, float] | None + safety_evidence_source: str + command_training_cycles: tuple[int, ...] = (0, 1, 2) + command_training_sample_ids: tuple[str, ...] = () + + +def _interval(value): + return len(value) == 2 and all(math.isfinite(v) for v in value) and value[0] < value[1] + + +def validate_range_evidence(name, original, delta, interval, evidence): + """Validate trusted fit evidence at both planning and final read-back.""" + if (not isinstance(evidence, MotionRangeEvidence) or evidence.joint != name + or evidence.source_limits != "calibrated_motion_envelope" or not evidence.evidence_source.strip() + or evidence.training_cycles != (0, 1, 2) or not evidence.training_sample_ids + or evidence.command_training_cycles not in {(0, 1, 2), (4,)} + or (evidence.command_training_cycles == (4,) and not evidence.command_training_sample_ids) + or not evidence.reference_sample_ids or not evidence.source_joint + or evidence.independently_measured != (evidence.source_joint == name) + or evidence.source_limits_cad_rad != (original.lower, original.upper) + or evidence.zero_offset_rad != delta + or not _interval(evidence.feedback_bounds_rad) or not _interval(evidence.command_bounds_rad)): + raise ValueError(f"invalid_training_motion_range_evidence:{name}") + expected = (min(0.0, evidence.feedback_bounds_rad[0], evidence.command_bounds_rad[0]), + max(0.0, evidence.feedback_bounds_rad[1], evidence.command_bounds_rad[1])) + if evidence.output_limits_rad != expected or not _interval(interval) \ + or any(abs(a-b) > 1e-10 for a, b in zip(interval, expected)): + raise ValueError(f"motion_range_differs_from_training_support:{name}") + safety = evidence.independent_safety_limits_cad_rad + if safety is None: + if evidence.safety_evidence_source: + raise ValueError(f"independent_safety_evidence_without_bounds:{name}") + elif (not _interval(safety) or not evidence.safety_evidence_source.strip() + or interval[0]+delta < safety[0]-1e-9 or interval[1]+delta > safety[1]+1e-9): + raise ValueError(f"measured_motion_exceeds_independent_safety_range:{name}") + + +def compile_motion_ranges(profile, model, fit): + """Freeze bounds from existing 3-cycle fitted mappings, without a refit. + + Copied curves retain donor image identities and never become independent + recipient measurements. All angular values already share the frozen zero. + """ + result = {} + from ..domain.sampling import command_training_cycles + command_cycles = command_training_cycles(profile) + for name, policy in profile.urdf_limit_policies.items(): + source = profile.measurement.transferred_motion_sources.get(name, name) + curve = fit.curves[source] + reference = fit.joint_zero_references[source] + command_evidence = fit.command_holdout_metrics[source] + if (tuple(curve.circle.get("training_cycles", ())) != (0, 1, 2) + or tuple(command_evidence.get("training_cycles", ())) != command_cycles + or not curve.circle.get("training_sample_ids") + or not reference.samples): + raise ValueError(f"motion_range_requires_training_provenance:{name}") + feedback, command = fit.output_mappings[name], fit.command_mappings[name] + if (feedback.transferred_from != (source if source != name else None) + or command.transferred_from != feedback.transferred_from + or abs(command.evaluate(reference.baseline_command)) > 1e-10): + raise ValueError(f"motion_range_requires_shared_baseline_and_donor:{name}") + bounds = (min(0.0, feedback.bounds_rad[0], command.bounds_rad[0]), + max(0.0, feedback.bounds_rad[1], command.bounds_rad[1])) + original = model.joints[name] + evidence = MotionRangeEvidence(name, source, source == name, + (original.lower, original.upper), bounds, feedback.bounds_rad, command.bounds_rad, + fit.zero_offsets_rad[name], reference.baseline_command, (0, 1, 2), + tuple(sorted(curve.circle["training_sample_ids"])), + tuple(sorted(f"{sample.view}:{sample.image_stamp_ns}" for sample in reference.samples)), + policy.source_limits, policy.evidence_source, policy.independent_safety_limits_cad_rad, + policy.safety_evidence_source, command_cycles, + tuple(command_evidence.get("training_sample_ids", ()))) + validate_range_evidence(name, original, fit.zero_offsets_rad[name], bounds, evidence) + result[name] = evidence + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py index 929b316..f79eb5c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py @@ -21,9 +21,15 @@ def validate_transferred_joint_tables(payload, measured_joints, transferred_moti or recipient["curve_input_knots_rad"] != donor["curve_input_knots_rad"] or baseline[recipient["motor_index"]] != baseline[donor["motor_index"]]): raise ValueError(f"transferred_final_table_differs_from_source:{target}") + for branch in ("increasing_rad", "decreasing_rad"): + if recipient.get(branch) != donor.get(branch): + raise ValueError(f"transferred_final_table_differs_from_source:{target}:{branch}") -def validate_measured_joint_holdout(payload, curves, observations, *, transferred_motion_sources=None): +def validate_measured_joint_holdout(payload, curves, observations, *, transferred_motion_sources=None, + holdout_cycle=3): + if holdout_cycle not in {3, 5}: + raise ValueError("command validation requires a declared independent partition") validate_transferred_joint_tables(payload, curves, transferred_motion_sources or {}) mapping = SerializedJointMapping(payload, input_kind="command") errors = {name: {d: [] for d in ("increasing", "decreasing")} for name in curves} @@ -31,13 +37,14 @@ def validate_measured_joint_holdout(payload, curves, observations, *, transferre unit = payload["input_unit"] for row in observations: name, identity = row["joint"], row["sample_id"] - if name not in curves or row["cycle"] != 3 or row.get("sample_phase") != "steady": + if name not in curves or row["cycle"] != holdout_cycle or row.get("sample_phase") != "steady": raise ValueError("v3 angle holdout has wrong partition or joint") if (name, identity) in seen or identity in curves[name].circle["training_sample_ids"]: raise ValueError("v3 angle holdout duplicates or overlaps training") seen.add((name, identity)) measured = measure_joint_curve_observation(curves[name], quaternion_xyzw=row["relative_quaternion_xyzw"]) - predicted = mapping.evaluate(row[f"command_vector_{unit}"], "", active_joints=(name,))[name] + predicted = mapping.evaluate(row[f"command_vector_{unit}"], + row["direction"] if name in mapping.directional_joints else "", active_joints=(name,))[name] errors[name][row["direction"]].append(predicted-measured) results = {} for name, branches in errors.items(): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py index 6189b7b..edeabcf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py @@ -2,7 +2,7 @@ Model profiles decide *what* values are authorized. This module owns the shared mechanics of locating those fields in the original XML text, changing -only the declared attributes, materializing mesh resources and atomically +only the declared attributes or explicit mimic removals, materializing mesh resources and atomically publishing a new file. """ @@ -16,7 +16,7 @@ from pathlib import Path import re import shutil import tempfile -from typing import Mapping, Sequence +from typing import Callable, Mapping, Sequence import xml.etree.ElementTree as ET import numpy as np @@ -25,13 +25,24 @@ from scipy.spatial.transform import Rotation @dataclass(frozen=True) class UrdfJointPatch: - """Authorized attribute replacements inside one top-level URDF joint.""" + """Attribute edits or explicit removal of one joint's linear mimic.""" origin_rpy: str | None = None limit_lower: str | None = None limit_upper: str | None = None mimic_multiplier: str | None = None mimic_offset: str | None = None + remove_mimic: bool = False + + def __post_init__(self) -> None: + if type(self.remove_mimic) is not bool or (self.remove_mimic and ( + self.mimic_multiplier is not None or self.mimic_offset is not None + )): + raise ValueError("mimic removal cannot also replace its coefficients") + + def requested_fields(self) -> tuple[str, ...]: + return tuple(f"{element}.{attribute}" for element, attribute, _ in self.replacements()) + ( + ("mimic.remove",) if self.remove_mimic else ()) def replacements(self) -> tuple[tuple[str, str, str], ...]: values = ( @@ -69,7 +80,7 @@ class UrdfPatchSet: def __post_init__(self) -> None: empty = [ - name for name, patch in self.joints.items() if not patch.replacements() + name for name, patch in self.joints.items() if not patch.requested_fields() ] if empty: raise ValueError( @@ -193,6 +204,12 @@ def apply_urdf_patch_text( block = match.group(0) for element, attribute, value in patch.replacements(): block = _replace_attribute(block, element, attribute, value) + if patch.remove_mimic: + if len(top_level_joints[name].findall("mimic")) != 1: + raise ValueError(f"mimic removal requires one original constraint:{name}") + block, count = re.subn(r"]*(?:/>|>\s*)", "", block) + if count != 1: + raise ValueError(f"could not uniquely remove mimic constraint:{name}") applied_joints.add(name) return block @@ -411,6 +428,7 @@ def write_urdf_patches( authorized_fields: Mapping[str, Sequence[str]] | None = None, validate_mimic_ranges: bool = True, expected_source_sha256: str | None = None, + validate_output: Callable[[Path], None] | None = None, ) -> Path: """Validate and atomically materialize one patched URDF.""" source = Path(source_urdf).expanduser().resolve() @@ -446,10 +464,7 @@ def write_urdf_patches( from .validate import validate_structural_urdf_diff requested_authorization = { - name: tuple( - f"{element}.{attribute}" - for element, attribute, _value in patch.replacements() - ) + name: patch.requested_fields() for name, patch in patches.joints.items() } authorization = ( @@ -481,6 +496,8 @@ def write_urdf_patches( # when calibration does not make it worse. if validate_mimic_ranges: validate_urdf_mimic_ranges(temporary, reference_urdf=source) + if validate_output is not None: + validate_output(temporary) materialize_relative_mesh_assets( source=source, output=destination.parent, urdf_root=root ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py index 7aefc5b..d945ce9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py @@ -13,6 +13,9 @@ from ..domain import CalibrationProfile from ..fitting.coupling import LinearMimicFit from .kinematics import UrdfKinematicModel, corrected_mimic_offset from .patch import UrdfJointPatch, UrdfPatchSet, corrected_origin_rpy, write_urdf_patches +from .limits import MotionRangeEvidence, validate_range_evidence +from .joint_control import independent_export_authorization, AFFINE_MIMIC_APPROXIMATION, PRESERVE_SOURCE_MIMIC +from .source_mimic import MimicEnvelope, compile_source_mimic_envelopes @dataclass(frozen=True) @@ -21,7 +24,8 @@ class StandardUrdfPlan: Limits are expressed in output coordinates. Mimic fits enter in CAD coordinates and are transformed exactly once here, including chains whose - parent is a passive joint. Nothing is inferred from JSON runtime curves. + parent is a passive joint. Current measured exports preserve source mimic + parameters as a standalone fallback; their JSON curves remain independent. """ source_sha256: str @@ -30,6 +34,10 @@ class StandardUrdfPlan: mimic_output: Mapping[str, tuple[str, float, float]] patches: UrdfPatchSet authorized_fields: Mapping[str, Sequence[str]] + range_evidence: Mapping[str, MotionRangeEvidence] = field(default_factory=dict) + independent_joint_sources: Mapping[str, str] | None = None + mimic_policy: str | None = None + mimic_envelopes: Mapping[str, MimicEnvelope] = field(default_factory=dict) def write(self, source_urdf: str | Path, destination_urdf: str | Path) -> Path: source = Path(source_urdf) @@ -50,22 +58,50 @@ def build_standard_correction_plan( fitted_mimics_cad: Mapping[str, LinearMimicFit] | None = None, transferred_zero_sources: Mapping[str, str] | None = None, require_shared_mimic_zero: bool = False, + range_evidence: Mapping[str, MotionRangeEvidence] | None = None, + independent_joint_channels: Mapping[str, int] | None = None, + fitted_mimics_output: Mapping[str, LinearMimicFit] | None = None, + preserve_source_mimic: bool = False, ) -> StandardUrdfPlan: """Generate authorized standard-URDF edits, never clamp an invalid fit. q_CAD = q_output + delta. Consequently both bounds shift by -delta and b_output = b_CAD + a*delta_parent - delta_child. Unobserved joints retain their own CAD geometry and physical range, including transferred joints. + An explicit training-supported motion envelope can replace nominal CAD + limits; separately declared hardware safety bounds remain mandatory. + With preserve_source_mimic, the original coefficients are kept verbatim + as an approximation in output coordinates, not rebased as a physical law. """ source = Path(source_urdf) if hashlib.sha256(source.read_bytes()).hexdigest() != source_sha256: raise ValueError("protected source URDF SHA256 mismatch") model = UrdfKinematicModel(source) + if preserve_source_mimic and (fitted_mimics_cad is not None or fitted_mimics_output is not None + or require_shared_mimic_zero or independent_joint_channels is not None): + raise ValueError("preserved_source_mimic_cannot_be_fitted_rebased_or_removed") + approximations = dict(fitted_mimics_output or {}) + if fitted_mimics_output is not None: + if fitted_mimics_cad or require_shared_mimic_zero or independent_joint_channels is not None: + raise ValueError("affine_mimic_export_requires_output_coordinate_approximations") + if set(approximations) != {name for name, joint in model.joints.items() if joint.mimic_joint}: + raise ValueError("affine_mimic_export_requires_all_source_couplings") + independent_sources = None + if independent_joint_channels is not None: + if fitted_mimics_cad or require_shared_mimic_zero: + raise ValueError("independent_joint_export_cannot_apply_linear_mimic_fits") + independent_sources = {name: joint.mimic_joint for name, joint in model.joints.items() + if joint.mimic_joint is not None} + authorized_fields = independent_export_authorization( + model, independent_joint_channels, independent_sources, authorized_fields) nodes = {str(node.get("name")): node for node in ET.parse(source).getroot().findall("joint")} offsets = {str(name): float(value) for name, value in zero_offsets_rad.items()} ranges = dict(measured_ranges_output_rad or {}) mimics = dict(fitted_mimics_cad or {}) - requested = set(offsets) | set(ranges) | set(mimics) + motion_ranges = dict(range_evidence or {}) + if not motion_ranges.keys() <= ranges.keys(): + raise ValueError("motion range evidence requires explicit output ranges") + requested = set(offsets) | set(ranges) | set(mimics) | set(approximations) if not requested <= model.joints.keys(): raise ValueError("correction references a missing URDF joint") for target, donor in (transferred_zero_sources or {}).items(): @@ -78,12 +114,17 @@ def build_standard_correction_plan( for name, value in offsets.items(): if not math.isfinite(value) or model.joints[name].kind not in {"revolute", "continuous"}: raise ValueError("zero corrections require finite revolute coordinates") + envelopes = {} + if preserve_source_mimic: + base_ranges = {name: tuple(ranges.get(name, (joint.lower-offsets.get(name, 0), + joint.upper-offsets.get(name, 0)))) for name, joint in model.joints.items() if joint.lower is not None} + envelopes = compile_source_mimic_envelopes(model, base_ranges, offsets, motion_ranges) edits: dict[str, UrdfJointPatch] = {} output_ranges: dict[str, tuple[float, float]] = {} output_mimics: dict[str, tuple[str, float, float]] = {} for name, joint in model.joints.items(): delta = offsets.get(name, 0.0) - fields: dict[str, str] = {} + fields: dict[str, str | bool] = {} if delta != 0: fields["origin_rpy"] = corrected_origin_rpy(nodes[name], delta) if joint.lower is not None: @@ -91,8 +132,14 @@ def build_standard_correction_plan( interval = tuple(float(v) for v in ranges.get(name, mechanical)) if len(interval) != 2 or not all(math.isfinite(v) for v in interval) or interval[0] >= interval[1]: raise ValueError(f"invalid corrected interval: {name}") - if interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9: + if name in motion_ranges: + validate_range_evidence(name, joint, delta, interval, motion_ranges[name]) + elif interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9: raise ValueError(f"corrected range exceeds source mechanical range: {name}") + if name in envelopes: + if interval != envelopes[name].base_limits_rad: + raise ValueError(f"source_mimic_base_range_differs_from_policy:{name}") + interval = envelopes[name].output_limits_rad output_ranges[name] = interval if interval[0] != joint.lower: fields["limit_lower"] = f"{interval[0]:.15g}" @@ -102,21 +149,24 @@ def build_standard_correction_plan( raise ValueError(f"cannot bound an unbounded joint without changing its contract: {name}") if name in mimics and joint.mimic_joint is None: raise ValueError(f"cannot create a new mimic topology: {name}") - if joint.mimic_joint is not None: - fit = mimics.get(name) + if joint.mimic_joint is not None and independent_sources is not None: + fields["remove_mimic"] = True + elif joint.mimic_joint is not None: + fit = approximations.get(name, mimics.get(name)) if fit is not None: if not isinstance(fit, LinearMimicFit) or fit.target_joint != name or fit.source_joint != joint.mimic_joint: raise ValueError(f"mimic fit does not match source URDF topology: {name}") if fit.training_cycles != (0, 1, 2) or not fit.residuals_rad or not all(math.isfinite(v) for v in fit.residuals_rad): raise ValueError(f"mimic fit lacks training-only evidence: {name}") - if not fit.offset_observed and not math.isclose(fit.offset_rad, joint.mimic_offset, rel_tol=0, abs_tol=1e-12): + if name not in approximations and not fit.offset_observed and not math.isclose(fit.offset_rad, joint.mimic_offset, rel_tol=0, abs_tol=1e-12): raise ValueError(f"unobservable passive static offset must retain CAD: {name}") multiplier, offset = fit.multiplier, fit.offset_rad else: multiplier, offset = joint.mimic_multiplier, joint.mimic_offset if not math.isfinite(multiplier) or not math.isfinite(offset): raise ValueError(f"nonfinite mimic fit: {name}") - shifted = corrected_mimic_offset(multiplier, offset, offsets.get(joint.mimic_joint, 0), delta) + shifted = offset if preserve_source_mimic or name in approximations else corrected_mimic_offset( + multiplier, offset, offsets.get(joint.mimic_joint, 0), delta) if require_shared_mimic_zero: if fit is None or not fit.offset_observed or abs(shifted) > 1e-12: raise ValueError(f"mimic lacks two supported baseline datums:{name}") @@ -129,7 +179,7 @@ def build_standard_correction_plan( if fields: patch = UrdfJointPatch(**fields) rights = set(authorized_fields.get(name, ())) - if any(f"{element}.{attribute}" not in rights for element, attribute, _ in patch.replacements()): + if not set(patch.requested_fields()) <= rights: raise ValueError(f"correction requests a field absent from Profile authorization: {name}") edits[name] = patch # Reject inconsistent full mimic ranges before creating any artifacts. @@ -155,7 +205,10 @@ def build_standard_correction_plan( for name in output_mimics: resolve(name) return StandardUrdfPlan(source_sha256, offsets, output_ranges, output_mimics, - UrdfPatchSet(edits), {name: tuple(fields) for name, fields in authorized_fields.items()}) + UrdfPatchSet(edits), {name: tuple(fields) for name, fields in authorized_fields.items()}, + motion_ranges, independent_sources, + PRESERVE_SOURCE_MIMIC if preserve_source_mimic else ( + AFFINE_MIMIC_APPROXIMATION if fitted_mimics_output is not None else None), envelopes) @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py index c523a65..4ee6d75 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py @@ -71,19 +71,17 @@ def prepare_measured_result(profile, model, source_sha256, fit): validate_mapping_provenance(profile, fit) validate_zero_provenance(profile, fit) passive = {name for name, joint in model.joints.items() if joint.mimic_joint} - if set(fit.mimic_approximations) != passive: - raise ValueError("independent_release_missing_paired_mimic_observations") - mimics = {name: evidence.in_cad_coordinates(fit.zero_offsets_rad) - for name, evidence in fit.mimic_approximations.items()} - # Keep legal mechanical limits for passive approximations; do not expand - # them to disguise an invalid linear reachable range. + # Measured support and the original linear fallback have separate envelopes. + # Only explicit nominal-limit policies permit expansion beyond CAD bounds. ranges = {name: (min(fit.output_mappings[name].bounds_rad[0], fit.command_mappings[name].bounds_rad[0], 0), max(fit.output_mappings[name].bounds_rad[1], fit.command_mappings[name].bounds_rad[1], 0)) for name in moving - passive} + from .limits import compile_motion_ranges + range_evidence = compile_motion_ranges(profile, model, fit) + ranges.update({name: evidence.output_limits_rad for name, evidence in range_evidence.items()}) plan = build_standard_correction_plan(source_urdf=model.source, source_sha256=source_sha256, zero_offsets_rad=fit.zero_offsets_rad, measured_ranges_output_rad=ranges, - fitted_mimics_cad=mimics, authorized_fields=profile.urdf_authorized_fields, require_shared_mimic_zero=True) - for name, (_, _, offset) in plan.mimic_output.items(): - if abs(offset) > 1e-12: - raise ValueError(f"mimic_does_not_pass_shared_zero:{name}") + authorized_fields=profile.urdf_authorized_fields, + preserve_source_mimic=True, + range_evidence=range_evidence) return PreparedCalibration(plan, dict(fit.output_mappings)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/source_mimic.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/source_mimic.py new file mode 100644 index 0000000..395371e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/source_mimic.py @@ -0,0 +1,120 @@ +"""Original CAD mimic fallback and its minimal representation envelopes. + +The measured JSON curves own actual joint motion. A retained CAD mimic is a +standalone approximation, including its original offset in output coordinates. +Its envelope is recorded separately from training evidence, never as additional +measured travel. Declared hardware bounds and default hard CAD bounds still apply. +""" + +from dataclasses import asdict, dataclass +import math +import xml.etree.ElementTree as ET + + +@dataclass(frozen=True) +class MimicEnvelope: + base_limits_rad: tuple[float, float] + mimic_reachable_limits_rad: tuple[float, float] + output_limits_rad: tuple[float, float] + + +def _resolve_envelopes(model, ranges, bases): + reachable, envelopes, visiting = {}, {}, set() + + def resolve(name): + if name in reachable: + return reachable[name] + if name in visiting: + raise ValueError(f"source_mimic_cycle:{name}") + if name not in model.joints: + raise ValueError(f"source_mimic_parent_missing:{name}") + visiting.add(name) + joint = model.joints[name] + if joint.mimic_joint is None: + if name not in ranges: + raise ValueError(f"source_mimic_requires_bounded_parent:{name}") + interval = ranges[name] + if (len(interval) != 2 or any(v is None or not math.isfinite(v) for v in interval) + or interval[0] >= interval[1]): + raise ValueError(f"invalid_source_mimic_parent_range:{name}") + else: + if not all(math.isfinite(v) for v in (joint.mimic_multiplier, joint.mimic_offset)): + raise ValueError(f"invalid_source_mimic_coefficients:{name}") + parent = resolve(joint.mimic_joint) + endpoints = [joint.mimic_multiplier*v+joint.mimic_offset for v in parent] + interval = (min(endpoints), max(endpoints)) + base = bases[name] + if len(base) != 2 or not all(math.isfinite(v) for v in base) or base[0] >= base[1]: + raise ValueError(f"invalid_source_mimic_base_range:{name}") + output = (min(base[0], interval[0]), max(base[1], interval[1])) + envelopes[name] = MimicEnvelope(base, interval, output) + visiting.remove(name) + reachable[name] = interval + return interval + + for name in bases: + resolve(name) + return envelopes + + +def compile_source_mimic_envelopes(model, ranges, offsets, motion_ranges): + """Compute from source coefficients and trusted ranges; never refit or pad.""" + bases = {} + for name, joint in model.joints.items(): + if joint.mimic_joint is None: + continue + if joint.lower is None: + raise ValueError(f"source_mimic_requires_bounded_child:{name}") + delta = offsets.get(name, 0.0) + bases[name] = (motion_ranges[name].output_limits_rad if name in motion_ranges else + (joint.lower-delta, joint.upper-delta)) + envelopes = _resolve_envelopes(model, ranges, bases) + for name, envelope in envelopes.items(): + evidence = motion_ranges.get(name) + delta = offsets.get(name, 0.0) + safety = (evidence.independent_safety_limits_cad_rad if evidence is not None else + (model.joints[name].lower, model.joints[name].upper)) + if safety is not None: + lo, hi = envelope.output_limits_rad + if lo+delta < safety[0]-1e-9 or hi+delta > safety[1]+1e-9: + raise ValueError(f"source_mimic_exceeds_safety_range:{name}:" + f"required_cad_rad=({lo+delta:.9g},{hi+delta:.9g}):" + f"allowed_cad_rad=({safety[0]:.9g},{safety[1]:.9g})") + return envelopes + + +def validate_source_mimic_elements(source, output): + """Even omitted coefficient attributes must stay omitted in new exports.""" + def elements(path): + return {node.get("name"): [ET.tostring(item, encoding="unicode").strip() + for item in node.findall("mimic")] + for node in ET.parse(path).getroot().findall("joint")} + if elements(source) != elements(output): + raise ValueError("exported_mimic_differs_from_source_urdf") + + +def validate_serialized_mimic_envelopes(payload, model): + """Check declared minimal envelopes against the actual exported model.""" + declared = payload["urdf_correction"].get("mimic_envelopes") + passive = {name for name, joint in model.joints.items() if joint.mimic_joint is not None} + if not isinstance(declared, dict) or set(declared) != passive: + raise ValueError("source_mimic_envelope_coverage_differs") + keys = {"base_limits_rad", "mimic_reachable_limits_rad", "output_limits_rad"} + for name, row in declared.items(): + if not isinstance(row, dict) or set(row) != keys: + raise ValueError(f"invalid_source_mimic_envelope:{name}") + for key, values in row.items(): + if (not isinstance(values, (list, tuple)) or len(values) != 2 + or any(type(v) not in (float, int) or not math.isfinite(v) for v in values) + or values[0] > values[1] + or (key != "mimic_reachable_limits_rad" and values[0] == values[1])): + raise ValueError(f"invalid_source_mimic_envelope:{name}:{key}") + ranges = {name: (j.lower, j.upper) for name, j in model.joints.items() if j.lower is not None} + expected = _resolve_envelopes(model, ranges, + {name: tuple(row["base_limits_rad"]) for name, row in declared.items()}) + for name, envelope in expected.items(): + for key, values in asdict(envelope).items(): + if any(abs(a-b) > 1e-10 for a, b in zip(values, declared[name][key])): + raise ValueError(f"source_mimic_envelope_differs:{name}:{key}") + if any(abs(a-b) > 1e-10 for a, b in zip(envelope.output_limits_rad, ranges[name])): + raise ValueError(f"source_mimic_envelope_differs_from_urdf:{name}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py index 3dc72af..ca4add6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py @@ -1,7 +1,8 @@ """Replay raw fourth-cycle Tag poses with the *written* URDF and JSON. -Mounting transforms and base registration are frozen from training. Passive -poses are resolved only through the exported standard URDF mimic graph. +Mounting transforms and base registration are frozen from training. Measured +v3 motion uses the complete JSON mapping, including nonlinear passive joints; +standard URDF mimic is evaluated separately as an approximation. """ from __future__ import annotations @@ -16,6 +17,10 @@ from scipy.spatial.transform import Rotation from ..fitting.tag_installation import FrozenTagInstallation, rigid_matrix from .acceptance import SerializedJointMapping, angular_metrics from .kinematics import UrdfKinematicModel +from .joint_control import independent_joint_sources + + +POSITION_P95_LIMIT_M = 0.003 @dataclass(frozen=True) @@ -36,7 +41,10 @@ def validate_serialized_tag_holdout( input_kind: str = "feedback", mimic_approximation: bool = False, enforce_precision: bool = True, + holdout_cycle: int = 3, ) -> dict[str, dict[str, Any]]: + if holdout_cycle not in {3, 5}: + raise ValueError("Tag validation requires a declared independent partition") model = UrdfKinematicModel(corrected_urdf) mapping = SerializedJointMapping(payload, input_kind=input_kind) base = rigid_matrix(common_from_base) @@ -45,14 +53,16 @@ def validate_serialized_tag_holdout( raise ValueError("final Tag holdout requires declared frozen installations") active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed" and joint.mimic_joint is None) - independent = mapping.measured and not mimic_approximation + # Full measured motion must use the JSON angles rather than the approximate + # mimic. Correction version 2 is the historical mimic-free export. + independent = mapping.measured and not mimic_approximation and independent_joint_sources(payload) is None if independent: active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed") training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids} seen = set() errors: dict[str, list[tuple[float, float]]] = {role: [] for role in roles} for row in observations: - if row.cycle != 3 or not row.sample_id or row.sample_id in training_ids: + if row.cycle != holdout_cycle or not row.sample_id or row.sample_id in training_ids: raise ValueError("final Tag holdout overlaps training or has no image identity") if row.role not in roles: raise ValueError("final Tag holdout contains an undeclared Tag") @@ -83,7 +93,7 @@ def validate_serialized_tag_holdout( raise ValueError(f"missing independent Tag holdout samples:{role}:{len(values)}") metrics = angular_metrics([value[0] for value in values]) translation_p95 = float(np.percentile([value[1] for value in values], 95)) - if enforce_precision and (not metrics.passed or translation_p95 > 0.003): + if enforce_precision and (not metrics.passed or translation_p95 > POSITION_P95_LIMIT_M): raise ValueError(f"serialized_urdf_tag_holdout_failed:{role}:mae_deg={metrics.mae_deg}:p95_deg={metrics.p95_deg}:max_deg={metrics.maximum_deg}:translation_p95_m={translation_p95}") results[role] = {"count": metrics.count, "mae_deg": metrics.mae_deg, "p95_deg": metrics.p95_deg, "maximum_deg": metrics.maximum_deg, diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py index 0aedf5a..987c127 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py @@ -47,7 +47,7 @@ def validate_structural_urdf_diff( field for fields in allowed.values() for field in fields - if field not in URDF_FIELD_TO_XML + if field not in URDF_FIELD_TO_XML and field != "mimic.remove" } if unsupported: raise ValueError( @@ -118,6 +118,11 @@ def validate_structural_urdf_diff( changes.append(f"{current_joint}.{field}") left_children = list(left) right_children = list(right) + if (left.tag == "joint" and path.count("/") == 2 + and "mimic.remove" in allowed.get(current_joint, ()) + and len(left.findall("mimic")) == 1 and not right.findall("mimic")): + left_children = [child for child in left_children if child.tag != "mimic"] + changes.append(f"{current_joint}.mimic.remove") if len(left_children) != len(right_children): raise ValueError(f"URDF topology changed at {path}") for index, (left_child, right_child) in enumerate( diff --git a/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py b/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py index 225b523..1cd7262 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py +++ b/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py @@ -231,6 +231,7 @@ class HikrobotCameraNode(Node): self._closed = False self._frame_count = 0 self._timeout_count = 0 + self._camera_timing = None self.selector = str(self.get_parameter("serial_number").value) self.expected_model = str(self.get_parameter("expected_model").value) @@ -286,6 +287,7 @@ class HikrobotCameraNode(Node): self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH) self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH) self.declare_parameter("grab_timeout_ms", 1000) + self.declare_parameter("timestamp_journal_path", "") def _load_calibration(self) -> CameraCalibration | None: if self.camera_info_path is None or not self.camera_info_path.exists(): @@ -407,6 +409,13 @@ class HikrobotCameraNode(Node): "set latest-frame grab strategy", camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly), ) + from .camera_timing import MvsCameraTiming + self._camera_timing = MvsCameraTiming(camera, mvs, + lambda: int(self.get_clock().now().nanoseconds), + exposure_us=0. if bool(self.get_parameter("auto_exposure").value) + else float(self.get_parameter("exposure_time_us").value), + journal_path=str(self.get_parameter("timestamp_journal_path").value)) + self._camera_timing.start() self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing()) self._grabbing = True self.get_logger().info( @@ -464,6 +473,11 @@ class HikrobotCameraNode(Node): assert self._mvs is not None timeout_ms = int(self.get_parameter("grab_timeout_ms").value) while not self._stop_event.is_set(): + try: + self._camera_timing.refresh_if_due() + except ValueError as error: + self.get_logger().error(f"Camera timestamp source failed: {error}") + return frame = self._mvs.MV_FRAME_OUT() memset(byref(frame), 0, sizeof(frame)) result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms) @@ -506,7 +520,9 @@ class HikrobotCameraNode(Node): raise RuntimeError( f"frame contains {info.nFrameLen} bytes, expected {expected_size}" ) - stamp = self.get_clock().now().to_msg() + from rclpy.time import Time + received_ns = int(self.get_clock().now().nanoseconds) + stamp = Time(nanoseconds=self._camera_timing.timestamp(info, received_ns)).to_msg() image = Image() image.header.stamp = stamp image.header.frame_id = self.frame_id @@ -596,6 +612,8 @@ class HikrobotCameraNode(Node): self._stop_event.set() if self._grab_thread is not None: self._grab_thread.join(timeout=2.0) + if self._camera_timing is not None: + self._camera_timing.close() if self._camera is not None and self._camera_opened: if self._grabbing: result = self._camera.MV_CC_StopGrabbing() diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py new file mode 100644 index 0000000..635e824 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py @@ -0,0 +1,31 @@ +"""Temporary diagnostic task selections, declared beside hand Profiles.""" + +from dataclasses import dataclass + +from ..core.domain.profile import ProfileKey + + +@dataclass(frozen=True) +class DiagnosticCapturePreset: + profile_key: ProfileKey + tasks: tuple[tuple[str, tuple[str, ...]], ...] + label_zh: str = "诊断采集" + hold_command_values: tuple[float, ...] = () + hold_seconds: float = 0.0 + + +DIAGNOSTIC_CAPTURE_PRESETS = { + "o6_thumb": DiagnosticCapturePreset( + ProfileKey("O6", "right", "o6_right_8", 1), + (("thumb_yaw_top", ("rh_thumb_cmc_yaw",)), + ("thumb_pitch_ip_front", ("rh_thumb_cmc_pitch", "rh_thumb_ip"))), + label_zh="拇指短程诊断", + ), + "o6_yaw_hold": DiagnosticCapturePreset( + ProfileKey("O6", "right", "o6_right_8", 1), + (("thumb_yaw_top", ("rh_thumb_cmc_yaw",)),), + label_zh="拇指yaw方向差保持诊断", + hold_command_values=(64.0, 128.0, 192.0), + hold_seconds=10.0, + ), +} diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py index cbdabe9..85b4674 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py @@ -51,6 +51,9 @@ def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]: "acquisition": _plain(asdict(profile.acquisition)), "urdf": { "authorized_fields": _plain(profile.urdf_authorized_fields), + **({"limit_policies": {name: _plain(asdict(policy)) + for name, policy in profile.urdf_limit_policies.items()}} + if profile.urdf_limit_policies else {}), }, "joint_coverage": _plain(profile.joint_coverage), } diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py index d331ca3..29c4e05 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py @@ -8,7 +8,7 @@ import re from typing import Any, Mapping import yaml -from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry +from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry, JointLimitPolicy from ..core import ( AcquisitionPolicy, @@ -56,7 +56,9 @@ def _validate_keys(payload): ("quality", QualityPolicy), ("scope", ScopePolicy), ("artifacts", ArtifactPolicy), ("acquisition", AcquisitionPolicy)): _keys(payload.get(name, {}), {f.name for f in fields(contract)}, name) - _keys(payload.get("urdf", {}), {"authorized_fields"}, "urdf") + _keys(payload.get("urdf", {}), {"authorized_fields", "limit_policies"}, "urdf") + for policy in payload.get("urdf", {}).get("limit_policies", {}).values(): + _keys(policy, {f.name for f in fields(JointLimitPolicy)}, "URDF limit policy") for view in payload.get("vision", {}).get("views", ()): _keys(view, {"name", "tags"}, "view") for tag in view.get("tags", ()): @@ -318,6 +320,7 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: str(artifacts.get("publication_pointer", "latest_passed")), _set(artifacts.get("session_compatibility_tokens")), bool(artifacts.get("publish_corrected_urdf", False)), + _set(artifacts.get("directional_command_joints")), ), acquisition=AcquisitionPolicy(**dict(acquisition)), sdk_adapter=str(payload["sdk_adapter"]), @@ -333,6 +336,12 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: payload.get("joint_coverage", {}), "joint_coverage" ).items() }, + urdf_limit_policies={str(name): JointLimitPolicy( + source_limits=str(row["source_limits"]), evidence_source=str(row["evidence_source"]), + independent_safety_limits_cad_rad=(None if row.get("independent_safety_limits_cad_rad") is None + else tuple(float(v) for v in row["independent_safety_limits_cad_rad"])), + safety_evidence_source=str(row.get("safety_evidence_source", ""))) + for name, row in _map(urdf.get("limit_policies", {}), "urdf.limit_policies").items()}, ) validate_profile(profile) return profile diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py index cccb03e..5d2934c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py @@ -42,3 +42,38 @@ def compile_tag_links(profile: CalibrationProfile, model: UrdfKinematicModel) -> raise ValueError(f"Tag is assigned to different kinematic branches:{tag.role}") bind(tag.role, model.joints[descendants[0]].child) return result + + +def compile_parallel_axis_geometry(profile, model, relations): + """Read invariant geometry only when both Tags identify adjacent links.""" + import numpy as np + from ..core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry + + links = compile_tag_links(profile, model) + by_child_role = {relation.child_role: relation for relation in relations} + result = [] + for child in relations: + parent = by_child_role.get(child.parent_role) + if parent is None: + continue + parent_name = profile.command.urdf_joint_by_joint.get(parent.joint, parent.joint) + child_name = profile.command.urdf_joint_by_joint.get(child.joint, child.joint) + first, second = model.joints[parent_name], model.joints[child_name] + if (first.kind not in {"revolute", "continuous"} or second.kind not in {"revolute", "continuous"} + or links[parent.child_role] != first.child or links[child.child_role] != second.child): + continue + chain = model._chain(child_name) + positions = [i for i, joint in enumerate(chain) if joint.name == parent_name] + if not positions or any(joint.kind != "fixed" for joint in chain[positions[0]+1:-1]): + continue + a, b = (model.link_transform(name, zero_offsets={}, joint_angles={}, independent_mimic_angles=True) + for name in (parent_name, child_name)) + axis_a, axis_b = a[:3, :3] @ first.axis, b[:3, :3] @ second.axis + if np.linalg.norm(np.cross(axis_a, axis_b)) > 1e-8: + continue + displacement = b[:3, 3] - a[:3, 3] + distance = float(np.linalg.norm(displacement - axis_a * (axis_a @ displacement))) + if distance > 1e-6: + result.append(ParallelAxisGeometry(parent.joint, child.joint, + 1 if axis_a @ axis_b > 0 else -1, distance)) + return tuple(result) diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py index 5a6adb1..758bef8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py @@ -6,7 +6,18 @@ from ..core.urdf.kinematics import UrdfKinematicModel from .observations import compile_tag_links +def require_release_geometry(report): + """Reject structurally impossible releases before acquiring any images.""" + coverage = report["independent_measurement_coverage"] + missing_motion = coverage["unresolved_motion_joints"] + missing_zero = coverage["missing_baseline_geometry"] + if missing_motion or missing_zero: + raise ValueError(f"release_geometry_incomplete:motion={missing_motion}:baseline={missing_zero}") + + def validate_executable_profile(profile, source_urdf): + if profile.artifacts.output_schema_version not in {2, 3}: + raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only") validate_profile(profile) if not profile.command.sdk_to_joint_direction: raise ValueError("executable Profile must declare fixed SDK-to-joint directions") @@ -26,8 +37,6 @@ def validate_executable_profile(profile, source_urdf): raise ValueError(f"unsupported measurement primitive:{name}:{spec.kind}") if not set(profile.zero.axis_joints) <= measured: raise ValueError("spatial observer axes require declared measured tasks") - if profile.artifacts.output_schema_version not in {2, 3}: - raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only") if profile.artifacts.output_schema_version == 3: required = {name for task in profile.motion.tasks for name in task.joints} if required != set(profile.motion.joint_zero_references): @@ -39,6 +48,9 @@ def validate_executable_profile(profile, source_urdf): _validate_transfer_geometry(profile, model) if not set(profile.urdf_authorized_fields) <= model.joints.keys(): raise ValueError("URDF authorization references a missing joint") + for name in profile.urdf_limit_policies: + if name not in model.joints or model.joints[name].lower is None: + raise ValueError(f"calibrated motion envelope requires an existing bounded CAD joint:{name}") conflicts = [] for name, joint in model.joints.items(): if joint.mimic_joint is None or joint.lower is None: @@ -71,6 +83,9 @@ def validate_executable_profile(profile, source_urdf): "cad_zero_assumptions": dict(profile.zero.cad_zero_assumptions), "transferred": dict(profile.zero.transferred_zero_sources), "cad_constraint_conflicts": conflicts, + "calibrated_motion_envelope_joints": sorted(profile.urdf_limit_policies), + "independent_angular_safety_limits_declared": sorted(name for name, policy + in profile.urdf_limit_policies.items() if policy.independent_safety_limits_cad_rad is not None), "observability": "structural_checks_only; numerical_rank_and_holdout_required", "arbitrary_multiaxis_validated": False} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py index 107aecc..8a8ece7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py @@ -14,6 +14,23 @@ from typing import Protocol, Sequence, runtime_checkable from ...core import CommandLayout +def ordered_feedback_values(layout, names, values): + """Apply the same channel order to positions and their measurement times.""" + if len(values) != layout.command_count: + return None + if not names or layout.feedback_by_index: + return tuple(values) + if len(names) != len(values) or len(set(names)) != len(names): + return None + by_name = dict(zip((str(name) for name in names), values)) + for alias, canonical in layout.feedback_name_aliases.items(): + if alias in by_name and canonical not in by_name: + by_name[canonical] = by_name[alias] + if any(name not in by_name for name in layout.names): + return None + return tuple(by_name[name] for name in layout.names) + + @dataclass(frozen=True) class HardwareHealth: connected: bool @@ -77,29 +94,13 @@ class ProfileSdkAdapter: def parse_feedback( self, names: Sequence[str], values: Sequence[float] ) -> tuple[float, ...] | None: - layout = self.command_layout - if len(values) != layout.command_count: + ordered = ordered_feedback_values(self.command_layout, names, values) + if ordered is None: return None - if names and not layout.feedback_by_index and ( - len(names) != len(values) or len(set(names)) != len(names) - ): + try: + parsed = tuple(float(value) for value in ordered) + except (ValueError, TypeError, OverflowError): return None - if names and not layout.feedback_by_index: - by_name = dict(zip((str(name) for name in names), values)) - for alias, canonical in layout.feedback_name_aliases.items(): - if alias in by_name and canonical not in by_name: - by_name[canonical] = by_name[alias] - if any(name not in by_name for name in layout.names): - return None - try: - parsed = tuple(float(by_name[name]) for name in layout.names) - except (ValueError, TypeError, OverflowError): - return None - else: - try: - parsed = tuple(float(value) for value in values) - except (ValueError, TypeError, OverflowError): - return None return parsed if all(math.isfinite(value) for value in parsed) else None def validate_command(self, values: Sequence[float]) -> tuple[float, ...]: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py index 0d82938..9e83166 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py @@ -25,6 +25,10 @@ class FinalizationInputs: source_urdf: Path protected_inputs: Mapping[str, str] records: tuple[dict[str, Any], ...] + require_motion_evidence: bool = False + journal_path: Path | None = None + journal_size: int | None = None + camera_extrinsics_file: Path | None = None class FinalizationProtocolError(RuntimeError): @@ -32,17 +36,29 @@ class FinalizationProtocolError(RuntimeError): class FinalizationController: - def __init__(self, profile, session_dir: Path, *, finalizer=finalize_profile_session): + def __init__(self, profile, session_dir: Path, *, finalizer=finalize_profile_session, isolated=False): self.worker = FinalizationWorker() self._events: SimpleQueue[str] = SimpleQueue() + self._isolated = isolated + if isolated: + from .process_finalizer import isolated_finalize_profile_session + finalizer = partial(isolated_finalize_profile_session, finalizer=finalizer) self._finalizer = partial(finalizer, profile=profile, phase_changed=self._events.put) self._publisher = ArtifactPublisher(session_dir.parent, profile.artifacts.publication_pointer) self.started = False def start(self, inputs: FinalizationInputs) -> None: + transport = {} + if inputs.camera_extrinsics_file is not None: + transport["camera_extrinsics_file"] = inputs.camera_extrinsics_file + if self._isolated: + if inputs.journal_path is None or inputs.journal_size is None: + raise ValueError("isolated_finalization_requires_synced_journal") + transport.update(journal_path=inputs.journal_path, journal_size=inputs.journal_size) self.worker.start(self._finalizer, session_dir=inputs.session_dir, serial_number=inputs.serial_number, source_urdf=inputs.source_urdf, - protected_inputs=dict(inputs.protected_inputs), records=inputs.records) + protected_inputs=dict(inputs.protected_inputs), records=inputs.records, + require_motion_evidence=inputs.require_motion_evidence, **transport) self.started = True def drain_events(self, on_event: Callable[[str], None]) -> None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py new file mode 100644 index 0000000..b2e807d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py @@ -0,0 +1,38 @@ +"""Direction state for tables measured on complete endpoint-to-endpoint sweeps.""" + +from math import floor + + +def next_input_directions(values, previous, directions, *, resolution, channels, byte_input=False): + """Pure transition: callers commit it only after every mapping check passes. + + A hold keeps its arrival direction. Initial state requires the recorded + baseline; an interior reversal needs unmeasured minor-loop hysteresis and + is rejected. An endpoint reset is the scope actually covered by calibration. + """ + result = list(directions) + for index, (baseline, lower, upper) in channels.items(): + if previous is None: + if values[index] != baseline: + raise ValueError(f"directional_mapping_requires_baseline_initialization:channel={index}") + if baseline not in (lower, upper): + raise ValueError("directional_mapping_requires_endpoint_baseline") + result[index] = "increasing" if baseline == upper else "decreasing" + if previous is not None: + for index, (old, new) in enumerate(zip(previous, values)): + tolerance = resolution + if byte_input and index in channels: + # The serialized byte table uses floor(v + .5). Compare the + # same effective inputs so small float steps cannot accumulate + # into a changed table index with a stale travel direction. + old, new = floor(old+.5), floor(new+.5) + tolerance = 0 + if abs(new-old) <= tolerance: + continue + direction = "increasing" if new > old else "decreasing" + if index in channels and direction != result[index]: + _, lower, upper = channels[index] + if min(abs(old-lower), abs(old-upper)) > tolerance: + raise ValueError(f"directional_mapping_interior_reversal_unvalidated:channel={index}") + result[index] = direction + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py index 5f73dab..9b7aaac 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py @@ -32,13 +32,42 @@ def _pose(value): return matrix_tuple(value) +def _replay_directions(mappings, row, task, *, directions_are_task_relative=False): + """A held ancestor retains its arrival branch in both replay paths.""" + direction = row["direction"] + if directions_are_task_relative and task.end_value > task.start_value: + direction = {"increasing": "decreasing", "decreasing": "increasing"}[direction] + if direction not in {"increasing", "decreasing"}: + raise ValueError("Tag replay requires an explicit scan direction") + # Older CaptureFrame records encode unavailable history as an empty + # vector. Preserve that explicit unknown state; a nonempty partial vector + # is damaged evidence and must not silently change only some branches. + history = row.get("command_direction_by_index") + if history is not None and not isinstance(history, (list, tuple)): + raise ValueError("Tag replay has an invalid SDK direction history") + if history: + covered = len(history) > max(m.motor_index for m in mappings.values()) + valid = all(value in {"", "increasing", "decreasing"} for value in history) + if not covered or not valid: + raise ValueError("Tag replay has an invalid SDK direction history") + directions = {} + for joint, mapping in mappings.items(): + if mapping.motor_index == task.command_index: + directions[joint] = direction + else: + directions[joint] = history[mapping.motor_index] if history else "" + return directions + + def prepare_command_holdout(profile, mappings, records): - """Raw steady fourth-cycle poses; no registration or mounting fit here.""" + """Raw independent command-validation poses; no registration or fit here.""" + from ...core.domain.sampling import command_holdout_cycle + holdout_cycle = command_holdout_cycle(profile) specs = profile.measurement.measurements tasks = {task.key: task for task in profile.motion.tasks} seen, result = set(), [] for row in records: - if row["cycle"] != 3: + if row["cycle"] != holdout_cycle: continue if row.get("sample_phase") != "steady": raise ValueError("command holdout requires stable command observations") @@ -48,13 +77,8 @@ def prepare_command_holdout(profile, mappings, records): if identity in seen: continue seen.add(identity) - directions = {joint: row["direction"] for joint, m in mappings.items() - if m.motor_index == task.command_index} - for joint, mapping in mappings.items(): - previous = row.get("command_direction_by_index", ()) - if mapping.motor_index != task.command_index and len(previous) > mapping.motor_index: - directions[joint] = previous[mapping.motor_index] - result.append(TagHoldout(row["sample_id"], spec.child_role, 3, + directions = _replay_directions(mappings, row, task) + result.append(TagHoldout(row["sample_id"], spec.child_role, holdout_cycle, tuple(row[f"command_vector_{profile.command.unit}"]), directions, _pose(row["child_pose_common"]))) return tuple(result) @@ -105,11 +129,8 @@ def prepare_tag_replay( input_domain = next(iter(domain)) field = f"state_{profile.command.unit}" if input_domain.startswith("feedback_") else f"command_vector_{profile.command.unit}" sdk = tuple(float(value) for value in row[field]) - direction = str(row["direction"]) - if directions_are_task_relative and task.end_value > task.start_value: - direction = {"increasing": "decreasing", "decreasing": "increasing"}[direction] - directions = {joint: direction for joint, mapping in output_mappings.items() - if mapping.motor_index == task.command_index} + directions = _replay_directions(output_mappings, row, task, + directions_are_task_relative=directions_are_task_relative) pose = _pose(row["child_pose_common"]) cycle = int(row["cycle"]) key = (role, identity) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py index b5fceac..3941ebb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py @@ -10,11 +10,13 @@ from ...core.artifacts.storage import atomic_write_json from ...core.fitting.session import fit_profile_calibration from ...core.fitting.command_mapping import fit_command_mappings from ...core.fitting.motion_fit import channel_for_joint +from ...core.fitting.rotation_diagnostics import RotationHoldoutFailure from ...core.geometry.candidate_selection import resolve_chain_observations from ...core.geometry.extrinsics import transform_matrix from ...core.urdf.kinematics import UrdfKinematicModel from ...core.urdf.result_plan import prepare_standard_result from ..acquisition import accepted_joint_records, accepted_secondary_records +from ..diagnostic_capture import reject_diagnostic_capture from .evidence import prepare_tag_replay from .publisher import ArtifactPublisher, FrozenTagArtifactValidator, StagedRelease from .serializers.compact_v2 import from_report, REPORT_FILENAME @@ -28,47 +30,96 @@ class CorrectedArtifact: staged_release: StagedRelease +def _write_artifact_pair(profile, fit, prepared, directory, source, serial_number, protected_inputs): + """Save the calibration first, then rebuild its URDF from those bytes. + + These are reviewable candidate files. Only independent final-file + acceptance and the publication controller can create a valid release. + """ + from .urdf_from_json import serialize_urdf_correction, rebuild_urdf_from_json + report_writer, command_writer = serialize_report, from_report + if profile.artifacts.output_schema_version >= 3: + from .serializers.measured_v3 import serialize_report as report_writer, from_report as command_writer + report = report_writer(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs) + report["urdf_correction"] = serialize_urdf_correction(prepared.plan) + if prepared.plan.range_evidence: + report["motion_range_evidence"] = {name: asdict(item) for name, item in prepared.plan.range_evidence.items()} + payload = command_writer(report) + naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()} + json_path = directory / profile.artifacts.calibration_filename.format(**naming) + urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming) + atomic_write_json(json_path, payload) + atomic_write_json(directory / REPORT_FILENAME, report) + rebuild_urdf_from_json(json_path, source, urdf_path, + authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) + return payload, json_path, urdf_path + + def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf, protected_inputs, records, publish=True, timestamp=None, standard_loader: Callable = validate_with_robot_state_publisher, cancelled: Callable[[], bool] = lambda: False, directions_are_task_relative: bool = False, + require_motion_evidence: bool = False, + camera_extrinsics_file=None, phase_changed: Callable[[str], None] = lambda _phase: None): """No worker may publish after an operator abort. The runtime owns cancellation synchronization. This function also checks immediately before publication and never returns the hand to a pose. """ + reject_diagnostic_capture(records) del timestamp if profile.artifacts.output_schema_version not in {2, 3}: raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only") + if profile.artifacts.output_schema_version >= 3: + from ...profiles.validator import validate_executable_profile, require_release_geometry + require_release_geometry(validate_executable_profile(profile, source_urdf)) directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve() directory.mkdir(parents=True, exist_ok=True) def check_cancelled(): if cancelled(): raise ValueError("operator_abort:no_artifact_publication") check_cancelled() + from ...core.fitting.command_sampling import validate_sampling_plans + sampling_plans = validate_sampling_plans(profile, records) + if sampling_plans: + atomic_write_json(directory / "command_sampling_plans.json", sampling_plans) + from ..motion_provenance import validate_source_geometry + validate_source_geometry(profile, source, records) if any(row.get("projection_reprocessing_scope") for row in records): raise ValueError("partial external projection is diagnostic-only; cannot certify a whole session") pose_reports = {} for task in profile.motion.tasks: if task.key not in profile.measurement.candidate_selection_tasks: continue - records, report = resolve_chain_observations(records, task_key=task.key, - fixed_roles={tag.role for view in profile.vision.views for tag in view.tags if tag.fixed_reference}, - role_pairs={name: (profile.measurement.measurements[name].parent_role, - profile.measurement.measurements[name].child_role) for name in task.joints}) + try: + records, report = resolve_chain_observations(records, task_key=task.key, + require_frozen_branches=True, + fixed_roles={tag.role for view in profile.vision.views for tag in view.tags if tag.fixed_reference}, + role_pairs={name: (profile.measurement.measurements[name].parent_role, + profile.measurement.measurements[name].child_role) for name in task.joints}) + except ValueError as error: + pose_reports[task.key] = {'status': 'failed', 'reason': str(error), 'publication_allowed': False} + atomic_write_json(directory / "pose_selection_diagnostics.json", pose_reports) + raise pose_reports[task.key] = report atomic_write_json(directory / "pose_selection_diagnostics.json", pose_reports) if report["status"] == "legacy_projection_unverified": raise ValueError("camera_projection_unverified:no_artifact_publication") + from ..motion_provenance import validate_motion_provenance + try: + motion_provenance = validate_motion_provenance(profile, records, + required=require_motion_evidence, require_complete=True) + except ValueError as error: + atomic_write_json(directory / "motion_branch_provenance.json", { + "status": "failed", "reason": str(error), "publication_allowed": False}) + raise + atomic_write_json(directory / "motion_branch_provenance.json", motion_provenance) accepted = accepted_joint_records(profile, records, directions_are_task_relative=directions_are_task_relative) secondary = accepted_secondary_records(profile, records, directions_are_task_relative=directions_are_task_relative) try: from ...core.domain.reference import read_joint_zero_references - if profile.artifacts.output_schema_version >= 3: - from ...profiles.validator import validate_executable_profile - validate_executable_profile(profile, source) references = read_joint_zero_references(profile, records) if profile.artifacts.output_schema_version >= 3 else None def save_relative_motion(motion): if profile.artifacts.output_schema_version >= 3: @@ -85,8 +136,11 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf from ...core.fitting.motion_transfer import expand_profile_transfers fit = expand_profile_transfers(profile, fit) except ValueError as error: - atomic_write_json(directory / "fit_diagnostics.json", { - "passed": False, "reason": str(error), "publication_allowed": False}) + diagnostic = {"passed": False, "reason": str(error), "publication_allowed": False} + if isinstance(error, RotationHoldoutFailure): + diagnostic.update(stage="relative_rotation_holdout", + rotation_holdout=asdict(error.diagnostics)) + atomic_write_json(directory / "fit_diagnostics.json", diagnostic) raise check_cancelled() phase_changed("fit_complete") @@ -96,53 +150,80 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf atomic_write_json(directory / "fit_diagnostics.json", {"passed": False, "reason": str(error), "publication_allowed": False, "relative_motion": {name: asdict(curve) for name, curve in fit.curves.items()}}) raise + check_cancelled() + phase_changed("holdout_complete") + payload, json_path, urdf_path = _write_artifact_pair( + profile, fit, prepared, directory, source, serial_number, protected_inputs) + phase_changed("artifacts_built") zero = fit.spatial_zero replay_records = [row for rows in accepted.values() for row in rows] replay_records.extend(dict(row, joint=profile.measurement.cross_view_sources[name]) for name, rows in secondary.items() for row in rows) - replay = prepare_tag_replay(profile=profile, source_model=UrdfKinematicModel(source), - plan=prepared.plan, output_mappings=fit.output_mappings, - common_from_base=transform_matrix(zero.base_translation_xyz_m, zero.base_quaternion_xyzw), - records=replay_records) + from ...core.fitting.tag_installation import TagInstallationFailure + try: + replay = prepare_tag_replay(profile=profile, source_model=UrdfKinematicModel(source), + plan=prepared.plan, output_mappings=fit.output_mappings, + common_from_base=transform_matrix(zero.base_translation_xyz_m, zero.base_quaternion_xyzw), + records=replay_records) + except TagInstallationFailure as error: + atomic_write_json(directory / "tag_installation_diagnostics.json", { + "passed": False, "publication_allowed": False, + "stage": "training_rigid_tag_installation", + "rotation_p95_limit_deg": 2.0, "translation_p95_limit_m": 0.003, + "roles": error.diagnostics}) + raise from .evidence import prepare_command_holdout + from ...core.domain.sampling import command_holdout_cycle + command_validation_cycle = command_holdout_cycle(profile) steady_secondary = accepted_secondary_records(profile, records, sample_phase="steady", directions_are_task_relative=directions_are_task_relative) steady_rows = [r for rows in steady.values() for r in rows] steady_rows.extend(dict(r, joint=profile.measurement.cross_view_sources[name]) for name, rows in steady_secondary.items() for r in rows) command_holdout = prepare_command_holdout(profile, fit.command_mappings, steady_rows) + from ..motion_provenance import uses_motion_evidence + image_inputs = {} + if require_motion_evidence or uses_motion_evidence(records): + if camera_extrinsics_file is None: + raise ValueError("final_image_holdout_requires_protected_camera_file") + from .image_evidence import ImageEvidence + image_evidence = ImageEvidence(profile, records, camera_extrinsics_file=camera_extrinsics_file, + protected_inputs=protected_inputs) + image_inputs = { + "image_observations": image_evidence.all_view_holdout(replay.holdout, + mappings=fit.output_mappings, input_kind="feedback", holdout_cycle=profile.quality.holdout_cycle), + "command_image_observations": image_evidence.all_view_holdout(command_holdout, + mappings=fit.command_mappings, input_kind="command", holdout_cycle=command_validation_cycle), + "require_image_holdout": True, + } validator = FrozenTagArtifactValidator(profile.key.profile_id, source, protected_inputs["source_urdf_sha256"], profile.urdf_authorized_fields, fit.zero_offsets_rad, replay.common_from_base, replay.installations, replay.holdout, replay.required_roles, standard_loader, protected_inputs=dict(protected_inputs), command_observations=command_holdout, - joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == 3), + joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == command_validation_cycle), + command_holdout_cycle=command_validation_cycle, joint_curves=fit.curves, joint_channels={name: channel_for_joint(profile, name) for name in fit.output_mappings}, transferred_motion_sources=profile.measurement.transferred_motion_sources, cad_zero_assumptions=profile.zero.cad_zero_assumptions, transferred_zero_sources=profile.zero.transferred_zero_sources, - joint_zero_methods=fit.zero_method_by_joint) - report_writer, command_writer = serialize_report, from_report - if profile.artifacts.output_schema_version >= 3: - from .serializers.measured_v3 import serialize_report as report_writer, from_report as command_writer - report = report_writer(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs) - payload = command_writer(report) - naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()} - json_path = directory / profile.artifacts.calibration_filename.format(**naming) - urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming) + joint_zero_methods=fit.zero_method_by_joint, range_evidence=prepared.plan.range_evidence, + directional_command_joints=profile.artifacts.directional_command_joints, **image_inputs) check_cancelled() - phase_changed("holdout_complete") - prepared.plan.write(source, urdf_path) - atomic_write_json(json_path, payload) - atomic_write_json(directory / REPORT_FILENAME, report) atomic_write_json(directory / "frozen_tag_installations.json", { "common_from_base": replay.common_from_base, "installations": {name: asdict(mount) for name, mount in replay.installations.items()}}) - phase_changed("artifacts_built") publisher = ArtifactPublisher(directory.parent, profile.artifacts.publication_pointer) - staged = publisher.prepare(session_directory=directory, calibration_json=json_path, - corrected_urdf=urdf_path, validate=validator, cancelled=cancelled) + from ...core.urdf.image_acceptance import ImageHoldoutFailure + try: + staged = publisher.prepare(session_directory=directory, calibration_json=json_path, + corrected_urdf=urdf_path, validate=validator, cancelled=cancelled) + except ImageHoldoutFailure as error: + atomic_write_json(directory / "final_image_diagnostics.json", { + "passed": False, "publication_allowed": False, "reason": str(error), + "input_kind": error.input_kind, "holdout_cycle": error.holdout_cycle, "roles": error.report}) + raise import json acceptance = json.loads(staged.validation_json) atomic_write_json(directory / "standard_urdf_acceptance.json", acceptance) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py new file mode 100644 index 0000000..c07fdd9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py @@ -0,0 +1,153 @@ +"""Bind final-file image checks to raw pixels and protected camera identities.""" + +import hashlib +import json +from numbers import Real +from pathlib import Path + +import numpy as np + +from ...core.geometry.extrinsics import camera_info_fingerprint, load_camera_extrinsics +from ...core.urdf.image_acceptance import TagImageObservation +from ..image_capture import IMAGE_OBSERVATION_POLICY +from .evidence import _replay_directions + + +class ImageEvidence: + """Validate source identities once, then prepare disjoint input holdouts. + + Primary observations retain their existing provenance checks. Additional + views contribute original pixels and synchronized SDK values directly, + without manufacturing a PnP pose or fitting against a holdout image. + """ + + def __init__(self, profile, records, *, camera_extrinsics_file, protected_inputs): + path = Path(camera_extrinsics_file) + if hashlib.sha256(path.read_bytes()).hexdigest() != protected_inputs["camera_extrinsics_sha256"]: + raise ValueError("final_image_camera_extrinsics_changed") + self.profile = profile + self.extrinsics = load_camera_extrinsics(path, required_views=profile.vision.view_names, + reference_view=profile.vision.extrinsic_reference_view, + quality_limits=profile.vision.extrinsics_quality_limits, + minimum_capture_counts=profile.vision.minimum_capture_counts) + self.models, self.frames, self.raw_frames, self.completed = {}, {}, {}, {} + self.tags = {tag.role: (view.name, tag) for view in profile.vision.views for tag in view.tags} + self.tasks = {task.key: task for task in profile.motion.tasks} + for row in records: + kind = row.get("kind") + if kind == "rectified_camera_model": + self._bind(self.models, row["view"], row) + elif kind in {"pnp_candidate_frame", "image_observation_frame"}: + target = self.frames if kind == "pnp_candidate_frame" else self.raw_frames + self._bind(target, f"{row['view']}:{row['image_stamp_ns']}", row) + elif kind == "scan_unit_complete": + key = (row["task_name"], row["cycle"], row["direction"]) + self.completed[key] = row + for view, model in self.models.items(): + identity = self.extrinsics.cameras[view] + fingerprint = camera_info_fingerprint(width=model["width"], height=model["height"], + camera_matrix=model["raw_k"], distortion=model["raw_d"], + rectification=model["rectification_r"], projection=model["projection_p"]) + if (fingerprint != identity.intrinsics_sha256 + or model.get("matrix_source") != "CameraInfo.P[:3,:3]" + or model.get("input_is_rectified") is not True): + raise ValueError("final_image_camera_intrinsics_changed") + if not np.array_equal(np.asarray(model["projection_p"]).reshape(3, 4)[:, :3], model["camera_matrix"]): + raise ValueError("final_image_camera_projection_changed") + + @staticmethod + def _bind(mapping, key, row): + if key in mapping and json.dumps(mapping[key], sort_keys=True) != json.dumps(row, sort_keys=True): + raise ValueError("final_image_source_identity_reused") + mapping[key] = row + + def _image(self, frame, *, sample_id, role, cycle, sdk_values, directions): + view, tag = self.tags[role] + model = self.models.get(view) + if (model is None or frame["view"] != view or frame["cycle"] != cycle + or frame.get("input_is_rectified") is not True + or frame.get("camera_matrix_source") != "CameraInfo.P[:3,:3]" + or not np.array_equal(frame["camera_matrix"], model["camera_matrix"])): + raise ValueError("final_image_holdout_source_missing_or_changed") + field = "tags" if frame["kind"] == "image_observation_frame" else "roles" + pixel = frame[field].get(role, {}) + if pixel.get("tag_size_m") != tag.size_m or pixel.get("tag_id") != tag.tag_id: + raise ValueError("final_image_holdout_tag_changed") + corners = np.asarray(pixel.get("corners_xy", ())) + if corners.shape != (4, 2) or not np.all(np.isfinite(corners)): + raise ValueError("final_image_holdout_corners_missing") + return TagImageObservation(sample_id, role, cycle, tuple(sdk_values), tuple(sorted(directions.items())), + view, tag.size_m, tuple(map(tuple, corners)), tuple(map(tuple, frame["camera_matrix"])), + tuple(map(tuple, np.linalg.inv(self.extrinsics.transform(view)))), + task_name=frame.get("task_name", "")) + + def primary_holdout(self, observations): + result = [] + for observation in observations: + frame = self.frames.get(observation.sample_id) + if frame is None: + raise ValueError("final_image_holdout_source_missing_or_changed") + result.append(self._image(frame, sample_id=observation.sample_id, role=observation.role, + cycle=observation.cycle, sdk_values=observation.sdk_values, + directions=observation.sdk_directions_by_joint)) + return tuple(result) + + def all_view_holdout(self, observations, *, mappings, input_kind, holdout_cycle): + """Use every synchronized image in the accepted independent scan units. + + Duplicate primary/raw identities must agree in both pixels and SDK + conditions. Missing feedback is explicit unavailable evidence; it is + never replaced with a command value or a fitted angle. + """ + if input_kind not in {"feedback", "command"}: + raise ValueError("final_image_input_kind_invalid") + primary = self.primary_holdout(observations) + expected = {(row.role, row.sample_id): row for row in primary} + roles = {row.role for row in primary} + phase = "sweep" if input_kind == "feedback" else "steady" + result = {} + count = self.profile.command.command_count + for sample_id, frame in self.raw_frames.items(): + if frame.get("cycle") != holdout_cycle or frame.get("sample_phase") != phase: + continue + unit = self.completed.get((frame["task_name"], holdout_cycle, frame["direction"])) + if unit is None or not unit["passed"] or unit["attempt"] != frame["attempt"]: + continue + if frame.get("policy") != IMAGE_OBSERVATION_POLICY or frame.get("command_unit") != self.profile.command.unit: + raise ValueError("final_image_raw_policy_or_unit_changed") + vectors = [frame.get("command_vector"), frame.get("feedback_vector")] + skew = frame.get("state_image_sync_error_ns") + if any(values is None for values in vectors) or skew is None: + continue + if (not isinstance(skew, Real) or isinstance(skew, bool) + or not np.isfinite(skew) or skew < 0): + raise ValueError("final_image_raw_sync_error_invalid") + arrays = [np.asarray(values) for values in vectors] + if any(values.shape != (count,) or values.dtype.kind not in "fiu" + or not np.all(np.isfinite(values)) for values in arrays): + raise ValueError("final_image_raw_sdk_vector_invalid") + history = frame.get("command_direction_by_index") + if (not isinstance(history, (list, tuple)) or len(history) != count + or any(value not in {"", "increasing", "decreasing"} for value in history)): + raise ValueError("final_image_raw_direction_history_invalid") + task = self.tasks.get(frame["task_name"]) + if task is None: + raise ValueError("final_image_raw_task_unknown") + directions = _replay_directions(mappings, frame, task) + values = frame["feedback_vector" if input_kind == "feedback" else "command_vector"] + for role in sorted(roles.intersection(frame["tags"])): + image = self._image(frame, sample_id=sample_id, role=role, cycle=holdout_cycle, + sdk_values=values, directions=directions) + identity = (role, sample_id) + if identity in expected and image != expected[identity]: + raise ValueError("final_image_primary_raw_disagree") + result[identity] = image + if not expected or not expected.keys() <= result.keys(): + raise ValueError("final_image_all_view_source_missing") + return tuple(result.values()) + + +def prepare_image_holdout(profile, records, observations, *, camera_extrinsics_file, protected_inputs): + """Historical primary-image binding; no new evidence is imputed to old files.""" + return ImageEvidence(profile, records, camera_extrinsics_file=camera_extrinsics_file, + protected_inputs=protected_inputs).primary_holdout(observations) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py new file mode 100644 index 0000000..ba37f90 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py @@ -0,0 +1,102 @@ +"""Isolate CPU-heavy finalization from live ROS callbacks. + +The parent thread transports events/results and owns cancellation. Only the +controller may publish. A bounded, synced journal prefix avoids pickling the +entire capture while holding the control lock. +""" + +import json +import multiprocessing +from pathlib import Path +import time +import traceback + +from .finalization import finalize_profile_session + + +def read_journal_prefix(path, size): + """Decode the frozen prefix without retaining copies of the raw journal. + + Captures can contain hundreds of megabytes of JSONL. Reading the whole + prefix and then splitting it keeps two additional copies alive while the + decoded records accumulate, alongside the live camera processes. + """ + if not isinstance(size, int) or isinstance(size, bool) or size <= 0: + raise ValueError("invalid_finalization_journal_size") + rows = [] + remaining = size + with Path(path).open("rb") as stream: + while remaining: + line = stream.readline(remaining) + if not line or not line.endswith(b"\n"): + raise ValueError("incomplete_finalization_journal_prefix") + remaining -= len(line) + row = json.loads(line) + if not isinstance(row, dict): + raise ValueError("invalid_finalization_journal_rows") + rows.append(row) + return tuple(rows) + + +def _prepare(connection, finalizer, journal_path, journal_size, kwargs): + started = time.monotonic() + try: + records = read_journal_prefix(journal_path, journal_size) + result = finalizer(**kwargs, records=records, publish=False, + phase_changed=lambda phase: connection.send(("phase", phase))) + connection.send(("result", result)) + except Exception: + connection.send(("error", traceback.format_exc())) + finally: + from ...core.artifacts.storage import atomic_write_json + atomic_write_json(Path(kwargs["session_dir"]) / "finalization_timing.json", { + "backend": "isolated_process", "elapsed_seconds": time.monotonic()-started, + "journal_prefix_bytes": journal_size}) + connection.close() + + +def isolated_finalize_profile_session(*, journal_path, journal_size, records, + phase_changed, cancelled, publish=False, finalizer=finalize_profile_session, + timeout_seconds=600., **kwargs): + """Same production finalizer, with bounded cancellation and no publication.""" + del records # The synced journal is the immutable capture transport. + if publish: + raise ValueError("isolated_worker_cannot_publish") + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + context = multiprocessing.get_context("spawn") + receive, send = context.Pipe(duplex=False) + process = context.Process(target=_prepare, + args=(send, finalizer, journal_path, journal_size, kwargs), daemon=True) + started = time.monotonic() + try: + process.start() + send.close() + while not cancelled(): + if receive.poll(.05): + kind, value = receive.recv() + if kind == "phase": + phase_changed(value) + elif kind == "result": + return value + elif kind == "error": + raise RuntimeError(f"finalization_process_failed:\n{value}") + else: + raise RuntimeError(f"invalid_finalization_process_event:{kind}") + elif not process.is_alive(): + raise RuntimeError(f"finalization_process_exited:{process.exitcode}") + if time.monotonic()-started >= timeout_seconds: + raise TimeoutError("finalization_process_timeout:no_artifact_publication") + raise ValueError("operator_abort:no_artifact_publication") + finally: + receive.close() + send.close() + if process.pid is not None: + process.join(timeout=.2) + if process.is_alive(): + process.terminate() + process.join(timeout=1.) + if process.is_alive(): + process.kill() + process.join(timeout=1.) + process.close() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py index 8ec10af..4702409 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py @@ -109,6 +109,12 @@ class FrozenTagArtifactValidator: cad_zero_assumptions: Mapping[str, str] = field(default_factory=dict) transferred_zero_sources: Mapping[str, str] = field(default_factory=dict) joint_zero_methods: Mapping[str, str] = field(default_factory=dict) + range_evidence: Mapping = field(default_factory=dict) + directional_command_joints: frozenset[str] = frozenset() + command_holdout_cycle: int = 3 + image_observations: tuple = () + command_image_observations: tuple = () + require_image_holdout: bool = False def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]: payload = json.loads(json_path.read_text(encoding="utf-8")) @@ -121,6 +127,8 @@ class FrozenTagArtifactValidator: if (measured_from_report if measured else from_report)(feedback_payload) != payload: raise ValueError("compact JSON differs from the frozen calibration report") if measured: + if payload.get("directional_command_joints", []) != sorted(self.directional_command_joints): + raise ValueError("directional command declaration differs from the Profile") if feedback_payload.get("protected_inputs") != dict(self.protected_inputs): raise ValueError("v3 report protected inputs differ from the session") if {name: row["sdk_channel"] for name, row in payload["joints"].items()} != dict(self.joint_channels): @@ -129,11 +137,16 @@ class FrozenTagArtifactValidator: validate_artifact_structure(source_urdf=self.source_urdf, corrected_urdf=urdf_path, source_sha256=self.source_sha256, authorized_fields=self.authorized_fields, payload=feedback_payload, - zero_offsets_rad=self.zero_offsets_rad) + zero_offsets_rad=self.zero_offsets_rad, range_evidence=self.range_evidence) changes = validate_artifact_structure(source_urdf=self.source_urdf, corrected_urdf=urdf_path, source_sha256=self.source_sha256, authorized_fields=self.authorized_fields, payload=payload, - zero_offsets_rad=self.zero_offsets_rad) + zero_offsets_rad=self.zero_offsets_rad, range_evidence=self.range_evidence) + rebuilt_from_json = "urdf_correction" in payload + if rebuilt_from_json: + from .urdf_from_json import validate_json_urdf_correspondence + validate_json_urdf_correspondence(payload, self.source_urdf, urdf_path, + expected_profile_id=self.profile_id) metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, payload=feedback_payload, common_from_base=self.common_from_base, installations=self.installations, observations=self.observations, @@ -144,32 +157,64 @@ class FrozenTagArtifactValidator: command_metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, payload=payload, common_from_base=self.common_from_base, installations=self.installations, observations=self.command_observations, - required_roles=self.required_roles, minimum_samples=3, input_kind="command") + required_roles=self.required_roles, minimum_samples=3, input_kind="command", + holdout_cycle=self.command_holdout_cycle) except ValueError as error: if payload.get("format") == COMPACT_FORMAT: raise ValueError(f"compact_command_table_holdout_failed:{error}") from error raise + image_metrics = {} + image_policy = None + if self.require_image_holdout: + from ...core.urdf.image_acceptance import FINAL_IMAGE_ACCEPTANCE_POLICY, validate_serialized_image_holdout + image_policy = FINAL_IMAGE_ACCEPTANCE_POLICY + common = dict(corrected_urdf=urdf_path, common_from_base=self.common_from_base, + installations=self.installations, required_roles=self.required_roles) + image_metrics["feedback"] = validate_serialized_image_holdout(**common, + payload=feedback_payload, observations=self.image_observations, + independent_observations=self.observations, + input_kind="feedback", holdout_cycle=3, minimum_samples=self.minimum_samples) + image_metrics["command"] = validate_serialized_image_holdout(**common, + payload=payload, observations=self.command_image_observations, + independent_observations=self.command_observations, + input_kind="command", holdout_cycle=self.command_holdout_cycle, minimum_samples=3) self.standard_loader(urdf_path) evidence = {"base": self.common_from_base, "installations": {k: asdict(v) for k, v in self.installations.items()}, "observations": [asdict(row) for row in self.observations]} evidence["steady_command_observations"] = [asdict(row) for row in self.command_observations] + evidence["image_observations"] = [asdict(row) for row in self.image_observations] + evidence["command_image_observations"] = [asdict(row) for row in self.command_image_observations] + evidence["motion_range_evidence"] = {name: asdict(item) for name, item in self.range_evidence.items()} extra = {} if measured: + from ...core.urdf.joint_control import ( + JSON_JOINT_ANGLES, independent_joint_sources, uses_affine_mimic_approximation, + uses_source_mimic, + ) from ...core.urdf.measured_acceptance import validate_measured_joint_holdout extra = {"independent_mapping_holdout": validate_measured_joint_holdout(payload, self.joint_curves, self.joint_observations, - transferred_motion_sources=self.transferred_motion_sources), + transferred_motion_sources=self.transferred_motion_sources, + holdout_cycle=self.command_holdout_cycle), "full_json_urdf_holdout": command_metrics, "independently_measured_joints": sorted(self.joint_curves), "transferred_joints": sorted(self.transferred_motion_sources), "transferred_motion_sources": dict(self.transferred_motion_sources), "cad_zero_assumptions": dict(self.cad_zero_assumptions), - "precision_scope": "observed_joints_travel_directions_and_held_poses", - "urdf_mimic_approximation": validate_serialized_tag_holdout(corrected_urdf=urdf_path, + "directional_command_joints": sorted(self.directional_command_joints), + "precision_scope": "observed_joints_travel_directions_and_held_poses"} + if independent_joint_sources(payload) is not None: + extra["urdf_joint_angle_source"] = JSON_JOINT_ANGLES + else: + extra["urdf_mimic_approximation"] = validate_serialized_tag_holdout(corrected_urdf=urdf_path, payload=payload, common_from_base=self.common_from_base, installations=self.installations, observations=self.command_observations, required_roles=self.required_roles, minimum_samples=3, - input_kind="command", mimic_approximation=True, enforce_precision=False)} + input_kind="command", mimic_approximation=True, enforce_precision=False, + holdout_cycle=self.command_holdout_cycle) + if uses_affine_mimic_approximation(payload) or uses_source_mimic(payload): + extra["urdf_joint_angle_source"] = JSON_JOINT_ANGLES + extra["urdf_mimic_policy"] = payload["urdf_correction"]["mimic_policy"] evidence["joint_observations"] = self.joint_observations evidence["frozen_joint_curves"] = {key: asdict(value) for key, value in self.joint_curves.items()} evidence["transferred_motion_sources"] = dict(self.transferred_motion_sources) @@ -177,10 +222,16 @@ class FrozenTagArtifactValidator: acceptance_kind = ("measured_and_transferred_json_urdf_v3" if self.transferred_motion_sources else "independently_measured_json_urdf_v3") if measured else "serialized_standard_urdf_tag_replay_v1" return {"acceptance": acceptance_kind, + "final_file_image_holdout": image_metrics, + "final_file_image_holdout_policy": image_policy, + "final_file_image_holdout_verified": self.require_image_holdout, **extra, + "urdf_rebuilt_from_json": rebuilt_from_json, "profile_id": self.profile_id, "source_urdf_sha256": self.source_sha256, "protected_inputs": dict(self.protected_inputs), + "motion_range_evidence": evidence["motion_range_evidence"], "holdout_cycle": 3, "tag_holdout": metrics, + "command_holdout_cycle": self.command_holdout_cycle, "steady_command_tag_holdout": command_metrics, "final_file_spatial_replay": "passed", "holdout_evidence_sha256": hashlib.sha256(json.dumps(evidence, @@ -189,6 +240,11 @@ class FrozenTagArtifactValidator: def _validate_report_provenance(self, report): """The report cannot grant itself new transfer or CAD authorizations.""" + if report.get("quality", {}).get("command_holdout_cycle", 3) != self.command_holdout_cycle: + raise ValueError("v3 command validation partition differs from the Profile") + ranges = json.loads(json.dumps({name: asdict(item) for name, item in self.range_evidence.items()})) + if report.get("motion_range_evidence", {}) != ranges: + raise ValueError("v3 motion range evidence differs from the frozen training fit") if report.get("transferred_motion_sources", {}) != dict(self.transferred_motion_sources): raise ValueError("v3 transfer sources differ from the measured Profile") rows = report["joints"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py index 1c2f30a..91a18ee 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py @@ -7,13 +7,19 @@ from pathlib import Path from ...core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables from ...core.urdf.kinematics import UrdfKinematicModel +from ...core.urdf.joint_control import ( + JSON_JOINT_ANGLES, independent_joint_sources, uses_affine_mimic_approximation, + uses_source_mimic, +) from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, from_report from .serializers.measured_v3 import FORMAT as MEASURED_FORMAT, from_report as measured_from_report +from .direction import next_input_directions class UnifiedCommandMapper: def __init__(self, payload, corrected_urdf, *, input_kind="command", expected_side=None, report=None): self.measured = payload.get("format") == MEASURED_FORMAT + self.json_mimic_override = self.measured and independent_joint_sources(payload) is None compact = payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT} if payload.get("format") not in {"unified_calibration_v1", COMPACT_FORMAT, MEASURED_FORMAT}: raise ValueError("requires a unified calibration artifact") @@ -45,6 +51,13 @@ class UnifiedCommandMapper: self.feedback_name_aliases = dict(metadata.get("feedback_name_aliases", {})) self.previous = None self.directions = [""]*len(self.command_names) + self.directional_channels = {} + for name in self.mapping.directional_joints: + row = self.mapping.joints[name] + knots = row["curve_input_knots_rad"] + lower, upper = (knots[0], knots[-1]) if knots else (0., 255.) + index = row["motor_index"] + self.directional_channels[index] = (metadata["baseline_command"][index], lower, upper) def map_positions(self, positions, names=()): values = tuple(float(v) for v in positions) @@ -56,14 +69,12 @@ class UnifiedCommandMapper: raise ValueError("feedback names differ from the serialized SDK layout") named = dict(zip(names, values)) values = tuple(named[n] for n in self.command_names) - directions = self.directions.copy() - if self.previous is not None: - for i, (old, new) in enumerate(zip(self.previous, values)): - if abs(new-old) > (0.5 if self.input_domain.endswith("u8") else 1e-9): - directions[i] = "increasing" if new > old else "decreasing" + directions = next_input_directions(values, self.previous, self.directions, + resolution=0.5 if self.input_domain.endswith("u8") else 1e-9, + channels=self.directional_channels, byte_input=self.input_domain.endswith("u8")) active = self.mapping.evaluate(values, "", active_joints=self.active, directions_by_joint={name: directions[row["motor_index"]] for name, row in self.mapping.joints.items()}) - result = self.urdf.resolve_angles(active, independent_mimic_angles=self.measured) + result = self.urdf.resolve_angles(active, independent_mimic_angles=self.json_mimic_override) for name, angle in result.items(): joint = self.urdf.joints[name] if joint.lower is not None and not joint.lower-1e-9 <= angle <= joint.upper+1e-9: @@ -91,8 +102,20 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com if validation.get("final_file_spatial_replay") != "passed" or not validation.get("steady_command_tag_holdout"): raise ValueError("release lacks independent command and feedback URDF evidence") payload = json.loads(json_path.read_text()) + if "urdf_correction" in payload and validation.get("urdf_rebuilt_from_json") is not True: + raise ValueError("release lacks JSON-to-URDF correspondence validation") report = json.loads(artifact("calibration_report").read_text()) if payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT} else None if payload.get("format") == MEASURED_FORMAT: + if (independent_joint_sources(payload) is not None + and validation.get("urdf_joint_angle_source") != JSON_JOINT_ANGLES): + raise ValueError("release lacks explicit JSON joint angle acceptance") + if (uses_affine_mimic_approximation(payload) or uses_source_mimic(payload)) and ( + validation.get("urdf_mimic_policy") != payload["urdf_correction"]["mimic_policy"] + or validation.get("urdf_joint_angle_source") != JSON_JOINT_ANGLES + or not validation.get("urdf_mimic_approximation")): + raise ValueError("release lacks separate mimic approximation acceptance") + if validation.get("directional_command_joints", []) != payload.get("directional_command_joints", []): + raise ValueError("v3 release directional mapping differs from its acceptance") transfers = report.get("transferred_motion_sources", {}) expected_kind = "measured_and_transferred_json_urdf_v3" if transfers else "independently_measured_json_urdf_v3" observed = {name for name, row in report["joints"].items() if row.get("independently_measured") is True} @@ -109,5 +132,7 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com if "cad_zero_assumption" in row} if validation.get("cad_zero_assumptions", {}) != assumptions: raise ValueError("v3 release CAD assumptions differ from its report") + if validation.get("motion_range_evidence", {}) != report.get("motion_range_evidence", {}): + raise ValueError("v3 release motion range evidence differs from its report") return UnifiedCommandMapper(payload, urdf_path, input_kind=input_kind, expected_side=expected_side, report=report) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py index c039cd2..cf9b3ea 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py @@ -5,6 +5,7 @@ import hashlib import json from ..acquisition import load_capture +from ..diagnostic_capture import reject_diagnostic_capture from ..engine import CalibrationEngine from ..resume import ResumeVerifier, fingerprint_from_mapping from ..scan_quality import evaluate_capture_unit @@ -13,13 +14,14 @@ from .finalization import finalize_profile_session def validate_capture_provenance(profile, serial_number, hashes, records): """An offline journal cannot substitute today's hashes for old evidence.""" + reject_diagnostic_capture(records) headers = [row for row in records if row.get("kind") == "session_start"] references = [row for row in records if row.get("kind") == "fixed_base_reference_locked"] if len(headers) != 1 or len(references) != 1: raise ValueError("capture_provenance:requires_unique_session_and_locked_reference") header, reference = headers[0], references[0] engine = CalibrationEngine(profile) - if (not engine.resume_compatible(header) or header.get("serial_number") != serial_number + if (not engine.capture_compatible(header) or header.get("serial_number") != serial_number or header.get("curve_input_domain") != profile.curve_input_domain): raise ValueError("capture_provenance:obsolete_policy_or_coordinate_identity; fresh capture required") for key, value in hashes.items(): @@ -48,6 +50,8 @@ def validate_capture_provenance(profile, serial_number, hashes, records): def validate_capture_units(profile, records): """Offline and resumed data must satisfy the actual post-sweep policy.""" + from ...core.fitting.command_sampling import validate_sampling_plans + validate_sampling_plans(profile, records) first_spans = {} for unit in CalibrationEngine(profile).scan_units(): key = (unit.task_key, unit.cycle, unit.direction) @@ -79,5 +83,6 @@ def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool payload, _fit, correction = finalize_profile_session( profile=profile, session_dir=output, serial_number=config.serial_number, source_urdf=config.source_urdf, - protected_inputs=hashes, records=records, publish=publish) + protected_inputs=hashes, records=records, publish=publish, + camera_extrinsics_file=config.camera_extrinsics) print(f"离线回放完成：schema {payload['schema_version']}，URDF {correction.path}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py index 750610b..399add9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py @@ -53,6 +53,7 @@ def from_report(report): for name in sorted(report["joints"]): resolve(name) return {"format": FORMAT, "schema_version": 2, + **({"urdf_correction": report["urdf_correction"]} if "urdf_correction" in report else {}), **{key: report[key] for key in ("profile_id", "model", "side", "serial_number")}, "input_unit": unit, "joints": {name: rows[name] for name in sorted(rows)}} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py index 89c54a1..5c543f8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py @@ -4,6 +4,7 @@ from dataclasses import asdict import numpy as np from .unified_v1 import _mapping +from ....core.urdf.joint_control import PRESERVE_SOURCE_MIMIC, uses_source_mimic FORMAT = "unified_calibration_v3" REPORT_FORMAT = "unified_calibration_report_v3" @@ -11,6 +12,7 @@ ZERO_CONVENTION = "q_CAD=q_output+delta; joint_baseline:q_output=0" def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs): + from ....core.domain.sampling import command_training_cycles, command_holdout_cycle expected = set(prepared.mappings) transfers = dict(profile.measurement.transferred_motion_sources) observed = expected - transfers.keys() @@ -20,6 +22,9 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) if set(fit.joint_zero_references) != observed or set(fit.curves) != observed: raise ValueError("independent_measurement_or_reference_coverage_differs") rows = {} + independent_sources = getattr(prepared.plan, "independent_joint_sources", None) + preserved = getattr(prepared.plan, "mimic_policy", None) == PRESERVE_SOURCE_MIMIC + passive_sources = prepared.plan.mimic_output if independent_sources is None else independent_sources for name in sorted(expected): donor = transfers.get(name) if any(mapping.transferred_from != donor for mapping in @@ -29,7 +34,7 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) "zero_offset_rad": fit.zero_offsets_rad[name], "zero_method": fit.zero_method_by_joint[name], "command_to_rad": _mapping(fit.command_mappings[name]), "feedback_to_rad": _mapping(fit.output_mappings[name]), - "passive": name in prepared.plan.mimic_output} + "passive": name in passive_sources} if donor is None: row.update(zero_reference=fit.joint_zero_references[name].as_record(), applicability=fit.command_applicability[name], @@ -40,13 +45,26 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) baseline_command=profile.command.baseline_values[fit.command_mappings[name].motor_index], applicability={"scope": "transferred_from_declared_joint", "source_joint": donor, "recipient_independently_validated": False, "arbitrary_multiaxis_validated": False}) - if row["passive"]: + if row["passive"] and preserved: + parent, multiplier, offset = prepared.plan.mimic_output[name] + row["mimic_approximation"] = {"joint": parent, "multiplier": multiplier, "offset_rad": offset, + "usage": "source_urdf_linear_fallback", "physical_zero_correction": False} + elif row["passive"]: approximation = fit.mimic_approximations[name] if approximation.transferred_from_joint != donor: raise ValueError(f"mimic_transfer_differs_from_profile:{name}") - parent, multiplier, offset = prepared.plan.mimic_output[name] + if independent_sources is None: + parent, multiplier, offset = prepared.plan.mimic_output[name] + else: + parent = independent_sources[name] + multiplier, offset = approximation.output_fit.multiplier, approximation.output_fit.offset_rad row["mimic_approximation"] = {"joint": parent, "multiplier": multiplier, "offset_rad": offset, "evidence": asdict(approximation)} + if independent_sources is not None: + row["mimic_approximation"]["usage"] = "diagnostic_only_not_exported" + elif getattr(prepared.plan, "mimic_policy", None) is not None: + row["mimic_approximation"].update(usage="urdf_linear_approximation", + baseline_error_rad=offset, physical_zero_correction=False) if name in profile.zero.known_baseline_geometry: row["known_baseline_geometry"] = asdict(profile.zero.known_baseline_geometry[name]) if name in profile.zero.cad_zero_assumptions: @@ -55,6 +73,8 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) row["zero_transferred_from_joint"] = profile.zero.transferred_zero_sources[name] rows[name] = row return {"format": REPORT_FORMAT, "schema_version": 3, + **({"directional_command_joints": sorted(profile.artifacts.directional_command_joints)} + if profile.artifacts.directional_command_joints else {}), "profile_id": profile.key.profile_id, "model": profile.key.model, "side": profile.key.side, "serial_number": serial_number, "command_unit": profile.command.unit, "command_names": list(profile.command.names), "baseline_command": list(profile.command.baseline_values), @@ -63,6 +83,9 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) "coordinate_convention": ZERO_CONVENTION, "protected_inputs": dict(protected_inputs), "joints": rows, **({"transferred_motion_sources": transfers} if transfers else {}), "quality": {"training_cycles": [0, 1, 2], "holdout_cycle": 3, + "command_capture_mode": profile.acquisition.command_capture_mode, + "command_training_cycles": list(command_training_cycles(profile)), + "command_holdout_cycle": command_holdout_cycle(profile), "precision_scope": "observed_travel_directions_and_held_poses", "independently_measured_joints": sorted(observed), "transferred_joints": sorted(transfers), "final_file_acceptance": "release_manifest.json", "arbitrary_multiaxis_validated": False}} @@ -75,6 +98,9 @@ def from_report(report): mapping = SerializedJointMapping(report, input_kind="command") feedback = SerializedJointMapping(report, input_kind="feedback") unit, baseline, rows = report["command_unit"], report["baseline_command"], {} + directional = set(report.get("directional_command_joints", ())) + if not directional <= mapping.joints.keys(): + raise ValueError("directional_command_joint_missing") if len(baseline) != len(report["command_names"]) or not np.all(np.isfinite(baseline)): raise ValueError("joint_baseline_vector_differs_from_sdk_layout") transfers = report.get("transferred_motion_sources", {}) @@ -129,7 +155,7 @@ def from_report(report): right = {key: value for key, value in source[kind].items() if key != "channel_index"} if left != right: raise ValueError(f"transferred_curve_differs_from_source:{name}:{kind}") - if evidence.get("passive"): + if evidence.get("passive") and not uses_source_mimic(report): approximation = evidence.get("mimic_approximation", {}) source_approximation = source.get("mimic_approximation", {}).get("evidence", {}) expected_approximation = {**source_approximation, @@ -149,8 +175,17 @@ def from_report(report): row["angle_rad"] = np.interp(np.arange(256), knots, angles).tolist() else: row.update(input_values=knots.tolist(), angle_rad=angles.tolist()) + if name in directional: + for direction in ("increasing", "decreasing"): + values = np.asarray(data[direction + "_rad"]) + if abs(np.interp(baseline[index], knots, values)) > 1e-10: + raise ValueError(f"directional_joint_baseline_is_not_zero:{name}:{direction}") + row[direction + "_rad"] = (np.interp(np.arange(256), knots, values).tolist() + if unit == "u8" else values.tolist()) rows[name] = row return {"format": FORMAT, "schema_version": 3, + **({"urdf_correction": report["urdf_correction"]} if "urdf_correction" in report else {}), + **({"directional_command_joints": sorted(directional)} if directional else {}), **{key: report[key] for key in ("profile_id", "model", "side", "serial_number")}, "input_unit": unit, "baseline_command": baseline, "coordinate_convention": ZERO_CONVENTION, "joints": rows} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/urdf_from_json.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/urdf_from_json.py new file mode 100644 index 0000000..4fbc727 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/urdf_from_json.py @@ -0,0 +1,150 @@ +"""Persisted calibration JSON -> immutable-source URDF, without fitting.""" + +from dataclasses import asdict, fields +import hashlib +import json +import math +from pathlib import Path +import xml.etree.ElementTree as ET + +from ...core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables +from ...core.urdf.joint_control import ( + JSON_JOINT_ANGLES, independent_joint_sources, independent_export_authorization, + AFFINE_MIMIC_APPROXIMATION, uses_affine_mimic_approximation, + PRESERVE_SOURCE_MIMIC, uses_source_mimic, +) +from ...core.urdf.source_mimic import validate_source_mimic_elements +from ...core.urdf.kinematics import UrdfKinematicModel +from ...core.urdf.patch import ( + UrdfJointPatch, UrdfPatchSet, apply_urdf_patch_text, + write_urdf_patches, validate_urdf_mimic_ranges, +) + + +def serialize_urdf_correction(plan): + """Persist numeric edits and the explicit JSON angle-provider contract.""" + if plan.patches.mujoco_equalities: + raise ValueError("standard_calibration_cannot_rebuild_mujoco_extensions") + sources = plan.independent_joint_sources + affine = plan.mimic_policy == AFFINE_MIMIC_APPROXIMATION + preserved = plan.mimic_policy == PRESERVE_SOURCE_MIMIC + return {"schema_version": 4 if preserved else (3 if affine else (1 if sources is None else 2)), + "source_urdf_sha256": plan.source_sha256, + **({"joint_angle_source": JSON_JOINT_ANGLES, "mimic_policy": plan.mimic_policy} if affine or preserved else {}), + **({"mimic_envelopes": {name: {key: list(value) for key, value in asdict(item).items()} + for name, item in plan.mimic_envelopes.items()}} if preserved else {}), + **({"joint_angle_source": JSON_JOINT_ANGLES, "passive_joint_sources": dict(sources)} + if sources is not None else {}), + "joint_patches": {name: {key: value for key, value in asdict(patch).items() + if value is not None and not (key == "remove_mimic" and value is False)} + for name, patch in sorted(plan.patches.joints.items())}} + + +def _read_correction(payload, expected_profile_id): + if (not isinstance(payload, dict) + or payload.get("format") not in {"unified_calibration_v2", "unified_calibration_v3"} + or payload.get("profile_id") != expected_profile_id): + raise ValueError("calibration_json_profile_or_format_mismatch") + correction = payload.get("urdf_correction") + if not isinstance(correction, dict): + raise ValueError("calibration_json_missing_supported_urdf_correction") + sources = independent_joint_sources(payload) + keys = {"schema_version", "source_urdf_sha256", "joint_patches"} + if sources is not None: + keys.update({"joint_angle_source", "passive_joint_sources"}) + preserved = uses_source_mimic(payload) + if uses_affine_mimic_approximation(payload) or preserved: + keys.update({"joint_angle_source", "mimic_policy"}) + if preserved: + keys.add("mimic_envelopes") + if set(correction) != keys: + raise ValueError("calibration_json_missing_supported_urdf_correction") + digest = correction["source_urdf_sha256"] + if not isinstance(digest, str) or len(digest) != 64 or any(c not in "0123456789abcdef" for c in digest): + raise ValueError("invalid_calibration_source_sha256") + declared = correction["joint_patches"] + allowed = {field.name for field in fields(UrdfJointPatch)} + if sources is None: + allowed.remove("remove_mimic") + if preserved: + allowed -= {"mimic_multiplier", "mimic_offset"} + if not isinstance(declared, dict): + raise ValueError("invalid_calibration_joint_edits") + patches = {} + for name, values in declared.items(): + if not isinstance(values, dict) or not values or not set(values) <= allowed: + raise ValueError(f"invalid_calibration_joint_edits:{name}") + for key, value in values.items(): + if key == "remove_mimic": + if value is not True: + raise ValueError(f"invalid_calibration_mimic_removal:{name}") + continue + if not isinstance(value, str): + raise ValueError(f"calibration_edit_requires_numeric_text:{name}:{key}") + numbers = [float(item) for item in value.split()] + if len(numbers) != (3 if key == "origin_rpy" else 1) or not all(math.isfinite(v) for v in numbers): + raise ValueError(f"invalid_calibration_numeric_edit:{name}:{key}") + patches[name] = UrdfJointPatch(**values) + if sources is not None and {name for name, patch in patches.items() if patch.remove_mimic} != set(sources): + raise ValueError("calibration_mimic_removals_differ_from_joint_control") + return digest, UrdfPatchSet(patches) + + +def _export_authorization(payload, source_urdf, authorized_fields): + sources = independent_joint_sources(payload) + if sources is None: + return authorized_fields + mapping = SerializedJointMapping(payload, input_kind="command") + channels = {name: row["motor_index"] for name, row in mapping.joints.items()} + return independent_export_authorization(UrdfKinematicModel(source_urdf), channels, sources, authorized_fields) + + +def validate_json_urdf_correspondence(payload, source_urdf, corrected_urdf, *, expected_profile_id): + """Prove that the saved correction describes the exact final URDF bytes.""" + digest, patches = _read_correction(payload, expected_profile_id) + source_bytes = Path(source_urdf).read_bytes() + if hashlib.sha256(source_bytes).hexdigest() != digest: + raise ValueError("calibration_json_source_sha256_mismatch") + _export_authorization(payload, source_urdf, {}) + rebuilt = apply_urdf_patch_text(source_bytes.decode("utf-8"), ET.fromstring(source_bytes), patches) + if rebuilt.encode("utf-8") != Path(corrected_urdf).read_bytes(): + raise ValueError("calibration_json_correction_differs_from_urdf") + if uses_source_mimic(payload): + validate_source_mimic_elements(source_urdf, corrected_urdf) + validate_compact_urdf_tables(payload, UrdfKinematicModel(corrected_urdf)) + + +def rebuild_urdf_from_json(calibration_json, source_urdf, destination_urdf, *, + authorized_fields, expected_profile_id): + """Validate and atomically write only edits read from the saved JSON. + + Rebuilding checks source identity, declared field rights, topology, full + mimic ranges and JSON tables. It does not certify measurement precision or + publish a release; the normal final-file validator still owns those checks. + """ + payload = json.loads(Path(calibration_json).read_text(encoding="utf-8")) + digest, patches = _read_correction(payload, expected_profile_id) + authorization = _export_authorization(payload, source_urdf, authorized_fields) + def validate(path): + if uses_source_mimic(payload): + validate_source_mimic_elements(source_urdf, path) + validate_urdf_mimic_ranges(path) + validate_compact_urdf_tables(payload, UrdfKinematicModel(path)) + return write_urdf_patches(source_urdf=source_urdf, destination_urdf=destination_urdf, + patches=patches, authorized_fields=authorization, + expected_source_sha256=digest, validate_output=validate) + + +def main(args=None): + import argparse + from ...product import load_product_config + parser = argparse.ArgumentParser(description="读取标定 JSON，从受保护的原始 URDF 重建修正文件") + parser.add_argument("--config", required=True) + parser.add_argument("--calibration-json", required=True) + parser.add_argument("--output", required=True) + selected = parser.parse_args(args) + config = load_product_config(selected.config, check_can=False) + profile = config.calibration_contract.typed_profile + output = rebuild_urdf_from_json(selected.calibration_json, config.source_urdf, selected.output, + authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) + print(f"已按标定 JSON 重建：{output}。精度结论以原会话验收为准。") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_diagnostics.py new file mode 100644 index 0000000..59f6368 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_diagnostics.py @@ -0,0 +1,32 @@ +"""Small solver lifecycle records, separate from immutable measurement evidence.""" + +import hashlib +import json +import logging + +from ..core.artifacts.storage import append_jsonl_many + + +def record_solver_event(path, request, event, *, stamp_ns, monotonic_time, **details): + """Record even a discarded result without changing a finalized raw journal. + + These events diagnose wall time and commit gates. They never authorize a + branch, a zero or publication, and cannot substitute for source images. + """ + sources = json.dumps(dict(request.source_frame_hashes), sort_keys=True, + separators=(",", ":")).encode() + record = { + "event": event, "stamp_ns": stamp_ns, "monotonic_time": monotonic_time, + "task_name": request.task_name, "view": request.view, + "zero_joints": list(request.zero_joints), "session_epoch": request.session_epoch, + "motion_version": request.motion_version, "generation": request.generation, + "source_frames": len(request.frames), "image_geometry": request.image_geometry, + "source_hashes_sha256": hashlib.sha256(sources).hexdigest(), + "source_first_stamp_ns": request.frames[0].stamp_ns if request.frames else None, + "source_last_stamp_ns": request.frames[-1].stamp_ns if request.frames else None, + **details, + } + try: + append_jsonl_many(path, (record,), durable=False) + except OSError: + logging.getLogger(__name__).exception("Could not record motion solver lifecycle") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py new file mode 100644 index 0000000..2d083fe --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py @@ -0,0 +1,301 @@ +"""Bounded evidence from declared zero-arrival motion, owned by the coordinator. + +Mutation is performed under the session lock after its observation-version +gate. A request is an immutable snapshot; the expensive geometry solver runs +outside that lock. Nothing here moves hardware, fits a control curve, or uses +training/holdout observations to revise an existing reference. +""" + +from dataclasses import asdict, dataclass +import hashlib +import json +import math + +from ..core.geometry.tag_pose.motion_evidence import ( + FrozenMotionConstraint, MotionBranchResolution, MotionEvidenceFrame, + MotionRelation, RoleCandidates, resolve_motion_branches, +) +from ..core.geometry.tag_pose.types import SquareTagPose +from ..core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from ..core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry +from ..core.geometry.tag_pose.production_image_motion import ( + ImageMotionModel, ImageMotionResolution, resolve_image_motion, +) +from .scan_quality import observation_streams +from .motion_provenance import source_frame_sha256 +from ..core.geometry.tag_pose.image_model_selection import IMAGE_MODEL_SELECTION_POLICY + + +@dataclass(frozen=True) +class MotionBranchModel: + task_name: str + view: str + constraints: tuple[FrozenMotionConstraint, ...] + evidence_ids: tuple[tuple[str, str], ...] + image_model: ImageMotionModel | None = None + + +@dataclass(frozen=True) +class BranchInitializationRequest: + task_name: str + view: str + zero_joints: tuple[str, ...] + session_epoch: int + motion_version: int + generation: int + frames: tuple[MotionEvidenceFrame, ...] + relations: tuple[MotionRelation, ...] + existing_evidence_ids: tuple[tuple[str, str], ...] + moving_roles: tuple[str, ...] + source_frame_hashes: tuple[tuple[str, str], ...] = () + invalid_reason: str = "" + image_frames: tuple[ImageMotionFrame, ...] = () + image_geometry: bool = False + geometry_constraints: tuple[ParallelAxisGeometry, ...] = () + source_urdf_sha256: str = "" + + +def _pose(payload): + return SquareTagPose(tuple(payload["quaternion_xyzw"]), + tuple(payload["translation_xyz_m"]), float(payload["reprojection_error_px"])) + + +def _finite_report(value): + if isinstance(value, float) and not math.isfinite(value): + return None + if isinstance(value, dict): + return {key: _finite_report(item) for key, item in value.items()} + if isinstance(value, (list, tuple)): + return [_finite_report(item) for item in value] + return value + + +def solve_initialization(request: BranchInitializationRequest): + """Pure expensive operation; the caller must check request identity again.""" + if request.invalid_reason: + return MotionBranchResolution(False, request.invalid_reason) + if request.image_geometry: + return resolve_image_motion(request.image_frames, request.relations, + geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256) + return resolve_motion_branches(request.frames, request.relations) + + +def initialization_artifacts(request, resolution): + """Bind each newly established physical-Tag branch to its actual evidence.""" + image_model = resolution.model if isinstance(resolution, ImageMotionResolution) else None + constraints = () if isinstance(resolution, ImageMotionResolution) else resolution.constraints + record = { + "kind": "motion_branch_initialization", "schema_version": 1, + "status": "resolved" if resolution.resolved else "unresolved", + "task_name": request.task_name, "view": request.view, + "zero_joints": list(request.zero_joints), + "session_epoch": request.session_epoch, "motion_version": request.motion_version, + "source_image_stamps": [frame.stamp_ns for frame in request.frames], + "source_frame_hashes": dict(request.source_frame_hashes), + "reason": resolution.reason, + "constraints": [asdict(model) for model in constraints], + "hypotheses": _finite_report([asdict(item) for item in resolution.hypotheses]), + "evidence_ids": {}, "evidence_payloads": {}, + } + if image_model is not None: + payload = asdict(image_model) + record["frozen_image_model"] = payload + record["image_model_sha256"] = hashlib.sha256(json.dumps( + payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() + if request.image_geometry: + record["image_model_selection_policy"] = IMAGE_MODEL_SELECTION_POLICY + record["image_geometry_constraints"] = [asdict(item) for item in request.geometry_constraints] + record["source_urdf_sha256"] = request.source_urdf_sha256 + if not resolution.resolved: + return None, record + existing = dict(request.existing_evidence_ids) + for role in request.moving_roles: + if role in existing: + record["evidence_ids"][role] = existing[role] + continue + payload = {key: record[key] for key in ( + "task_name", "view", "zero_joints", "session_epoch", "motion_version", + "source_image_stamps", "source_frame_hashes", "constraints", + )} + if image_model is not None: + payload.update(frozen_image_model=record["frozen_image_model"], + image_model_sha256=record["image_model_sha256"]) + payload["tag_role"] = role + encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) + record["evidence_ids"][role] = hashlib.sha256(encoded.encode()).hexdigest() + record["evidence_payloads"][role] = payload + model = MotionBranchModel(request.task_name, request.view, constraints, + tuple(sorted(record["evidence_ids"].items())), image_model) + return model, record + + +class BranchInitialization: + """At most 129 command bins per view from the final approach segment.""" + + def __init__(self, profile, *, image_geometry=True, source_urdf=None): + self.profile = profile + self.image_geometry = image_geometry + self.source_model = None + self.source_urdf_sha256 = "" + if source_urdf is not None: + from ..core.urdf.kinematics import UrdfKinematicModel + self.source_model = UrdfKinematicModel(source_urdf) + self.source_urdf_sha256 = hashlib.sha256(self.source_model.source.read_bytes()).hexdigest() + self.generation = 0 + self.clear() + + def clear(self): + self.generation += 1 + self.task_name = None + self.zero_joints = () + self.motion_version = None + self.session_epoch = None + self._frames = {} + self._source_records = {} + self._existing_ids = {} + self._held_feedback = None + self._invalid_reason = "" + self._requested = set() + self.reports = {} + + def begin(self, motion, *, session_epoch, motion_version): + if motion.phase == "zero_approach": + self.clear() + self.task_name = motion.task_key + self.zero_joints = motion.reference_joints + self.motion_version = motion_version + self.session_epoch = session_epoch + elif motion.phase == "joint_zero": + if (self.task_name, self.zero_joints, self.session_epoch) != ( + motion.task_key, motion.zero_joints, session_epoch): + self.clear() + self.task_name, self.zero_joints = motion.task_key, motion.zero_joints + self.session_epoch = session_epoch + self._invalid_reason = "motion_approach_evidence_missing" + + def _relations(self, view): + task = next(task for task in self.profile.motion.tasks if task.key == self.task_name) + return tuple(MotionRelation(name, spec.parent_role, spec.child_role) + for _, name, spec in observation_streams(self.profile, task) + if name in self.zero_joints and spec.view == view) + + def observe(self, record): + if (record["task_name"], tuple(record["zero_joints"]), record["motion_version"], + record["session_epoch"]) != ( + self.task_name, self.zero_joints, self.motion_version, self.session_epoch): + return + command, feedback = record["command_vector"], record["feedback_vector"] + if command is None or feedback is None or not self.zero_joints: + return + spec = self.profile.motion.joint_zero_references[self.zero_joints[0]] + task = next(task for task in self.profile.motion.tasks if task.key == self.task_name) + index = task.command_index + # The current measured channel moves; every other declared command + # must already be in this reference's actual clearance/holding pose. + if any(abs(value - spec.command[i]) > 1e-9 + for i, value in enumerate(command) if i != index): + return + if self._held_feedback is None: + self._held_feedback = tuple(feedback) + layout = self.profile.command + for i, value in enumerate(feedback): + tolerance = max(1.0 if layout.unit == "u8" else 0.002, + 0.005 * (layout.maximum_feedback_values[i] - layout.minimum_feedback_values[i])) + if i != index and abs(value - self._held_feedback[i]) > tolerance: + self._invalid_reason = "motion_held_feedback_changed" + return + relations = self._relations(record["view"]) + if not relations: + return + roles = tuple(dict.fromkeys(role for relation in relations + for role in (relation.parent_role, relation.child_role))) + items, existing = [], self._existing_ids.setdefault(record["view"], {}) + for role in roles: + item = record["tags"].get(role) + if item is None: + return + diagnostics = item["candidate_diagnostics"] + if diagnostics.get("observation_stamp_ns") != record["image_stamp_ns"]: + return + frozen = diagnostics.get("branch_frozen") is True + if frozen: + if item["selected_pose"] is None: + return + candidates = (_pose(item["selected_pose"]),) + evidence_id = diagnostics.get("motion_evidence_id") + if evidence_id: + if existing.get(role, evidence_id) != evidence_id: + self._invalid_reason = "motion_frozen_evidence_changed" + return + existing[role] = evidence_id + else: + candidates = tuple(_pose(p) for p in item["reprojection_valid_candidates"]) + items.append(RoleCandidates(role, candidates, frozen)) + lower, upper = layout.minimum_values[index], layout.maximum_values[index] + bin_index = round(128 * (command[index] - lower) / (upper - lower)) + self._frames.setdefault(record["view"], {})[bin_index] = MotionEvidenceFrame( + record["image_stamp_ns"], tuple(items)) + self._source_records.setdefault(record["view"], {})[bin_index] = record + + def request(self, view, motion, *, session_epoch, motion_version): + if motion is None or motion.phase != "joint_zero" or view in self._requested: + return None + if (self.task_name, self.zero_joints, self.session_epoch) != ( + motion.task_key, motion.zero_joints, session_epoch): + return None + relations = self._relations(view) + if not relations: + return None + self._requested.add(view) + roles = {role for relation in relations for role in (relation.parent_role, relation.child_role)} + moving = tuple(sorted(tag.role for v in self.profile.vision.views if v.name == view + for tag in v.tags if tag.role in roles and not tag.fixed_reference)) + frames = tuple(sorted(self._frames.get(view, {}).values(), key=lambda frame: frame.stamp_ns)) + sources = tuple(self._source_records.get(view, {}).values()) + by_stamp = {row["image_stamp_ns"]: row for row in sources} + image_frames, reason = (), self._invalid_reason + if self.image_geometry: + try: + image_frames = tuple(ImageMotionFrame(frame, + tuple(tuple(float(v) for v in values) for values in by_stamp[frame.stamp_ns]["camera_matrix"]), + tuple(ImageRoleObservation(item.role, + float(by_stamp[frame.stamp_ns]["tags"][item.role]["tag_size_m"]), + tuple(tuple(float(v) for v in point) for point in + by_stamp[frame.stamp_ns]["tags"][item.role]["corners_xy"])) + for item in frame.roles)) for frame in frames) + except (KeyError, TypeError, ValueError): + reason = reason or "image_motion_preparation_pixels_missing" + geometry_constraints = () + if self.image_geometry and self.source_model is not None: + from ..profiles.observations import compile_parallel_axis_geometry + geometry_constraints = compile_parallel_axis_geometry(self.profile, self.source_model, relations) + return BranchInitializationRequest(self.task_name, view, self.zero_joints, + session_epoch, self.motion_version if self.motion_version is not None else motion_version, self.generation, + frames, + relations, tuple(sorted(self._existing_ids.get(view, {}).items())), moving, + tuple(sorted((str(row["image_stamp_ns"]), source_frame_sha256(row)) + for row in sources)), reason, image_frames, self.image_geometry, + geometry_constraints, self.source_urdf_sha256 if geometry_constraints else "") + + def pending(self): + """A bounded preparation solve is running outside the state lock.""" + return bool(self._requested - self.reports.keys()) + + def accept(self, request, resolution): + if request.generation != self.generation: + return None + model, report = initialization_artifacts(request, resolution) + self.reports[request.view] = report + return model, report + + def error(self): + for view, report in sorted(self.reports.items()): + if report["status"] != "resolved": + return f"view={view}:joints={','.join(self.zero_joints)}:{report['reason']}" + return "" + + def ready(self, motion): + if (motion.task_key, motion.zero_joints) != (self.task_name, self.zero_joints): + return False + views = [view.name for view in self.profile.vision.views if self._relations(view.name)] + return bool(views) and all(self.reports.get(view, {}).get("status") == "resolved" for view in views) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py new file mode 100644 index 0000000..4f252c9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py @@ -0,0 +1,94 @@ +"""Apply an immutable task-local model to actual same-image Tag candidates.""" + +from ..core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates, select_motion_frame +from ..core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from ..core.geometry.tag_pose.tracking import MotionPoseSelection, SquareTagPoseTracker + + +class CommittedPoseEvidence: + """Read the metadata copied atomically with this frame's joint decision.""" + + def __init__(self, tracker, managed_roles, snapshots): + self.tracker = tracker + self.managed_roles = frozenset(managed_roles) + self.snapshots = {item.role: item for item in snapshots} + + def observation_snapshot(self, role, stamp_ns): + if role not in self.managed_roles: + return self.tracker.observation_snapshot(role, stamp_ns) + item = self.snapshots.get(role) + if item is None or item.stamp_ns != stamp_ns: + return None + return item.candidates, dict(item.diagnostics), item.corrections + + +def select_motion_poses(frame, model, tracker, selected, *, base, cached, projection_worker=None): + """Called outside the session lock; tracker commits check image versions.""" + if model.image_model is not None: + return _select_image_poses(frame, model, tracker, selected, projection_worker=projection_worker) + ids = dict(model.evidence_ids) + roles = tuple(dict.fromkeys(role for constraint in model.constraints + for role in (constraint.relation.parent_role, constraint.relation.child_role))) + snapshots, candidates = {}, [] + for role in roles: + if role in ids: + snapshot = tracker.motion_snapshot(role, frame.stamp_ns) + if snapshot is not None and snapshot.motion_managed: + snapshots[role] = snapshot + poses = () if snapshot is None else snapshot.candidates + candidates.append(RoleCandidates(role, poses)) + else: + pose = selected.get(role) + if (pose is None and role == base.role and cached is not None + and role not in frame.corners and role not in frame.observed_roles): + pose = cached[0] # Same verified stationary-cache contract as capture. + candidates.append(RoleCandidates(role, () if pose is None else (pose,), frozen=True)) + decision = select_motion_frame(model.constraints, candidates) + if not decision.resolved or set(snapshots) != set(ids): + reason = "pose_branch_" + (decision.reason if not decision.resolved else "motion_candidates_missing") + rejected = SquareTagPoseTracker.reject_motion_frame(tuple(snapshots.values()), reason) + evidence = CommittedPoseEvidence(tracker, ids, rejected.observation_snapshots) + return {}, {role: rejected.reason for role in ids}, evidence + proposals = tuple(MotionPoseSelection(snapshots[item.role], item.pose, ids[item.role]) + for item in decision.selections if item.role in ids) + result = SquareTagPoseTracker.commit_motion_frame(proposals, + establish=any(snapshot.evidence_id is None for snapshot in snapshots.values())) + evidence = CommittedPoseEvidence(tracker, ids, result.observation_snapshots) + if not result.accepted: + return {}, {role: result.reason for role in ids}, evidence + return {item.snapshot.role: item.pose for item in result.selections}, {}, evidence + + +def _select_image_poses(frame, model, tracker, selected, *, projection_worker=None): + """The model owns its children; already frozen parents keep their identity.""" + image_model = model.image_model + children = {item.relation.child_role for item in image_model.geometry} + ids = {role: identity for role, identity in model.evidence_ids if role in children} + snapshots, candidates, observations = {}, [], [] + for role, size in image_model.tag_sizes: + if role not in frame.corners: + continue + observations.append(ImageRoleObservation(role, size, + tuple(tuple(float(v) for v in point) for point in frame.corners[role]))) + if role in children: + snapshot = tracker.motion_snapshot(role, frame.stamp_ns) + if snapshot is not None and snapshot.motion_managed: + snapshots[role] = snapshot + candidates.append(RoleCandidates(role, snapshot.candidates)) + elif role in selected: + candidates.append(RoleCandidates(role, (selected[role],), frozen=True)) + image_frame = ImageMotionFrame(MotionEvidenceFrame(frame.stamp_ns, tuple(candidates)), + tuple(tuple(float(v) for v in row) for row in frame.camera_matrix), tuple(observations)) + decision = SquareTagPoseTracker.verify_image_motion_frame(tuple(snapshots.values()), + image_model, image_frame, ids, projection_worker=projection_worker) + if not decision.accepted: + reason = "pose_branch_" + decision.reason + rejected = SquareTagPoseTracker.reject_motion_frame(tuple(snapshots.values()), reason) + evidence = CommittedPoseEvidence(tracker, children, rejected.observation_snapshots) + return {}, {role: rejected.reason for role in children}, evidence + result = SquareTagPoseTracker.commit_image_motion_frame(decision, + establish=any(snapshot.evidence_id is None for snapshot in snapshots.values())) + evidence = CommittedPoseEvidence(tracker, children, result.observation_snapshots) + if not result.accepted: + return {}, {role: result.reason for role in children}, evidence + return {item.snapshot.role: item.pose for item in result.selections}, {}, evidence diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_worker.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_worker.py new file mode 100644 index 0000000..6c1a137 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_worker.py @@ -0,0 +1,73 @@ +"""Run immutable geometry requests outside the ROS interpreter and its GIL.""" + +import multiprocessing +import threading +import time +import traceback + +from .branch_initialization import solve_initialization +from .branch_diagnostics import record_solver_event +from ..core.geometry.tag_pose.motion_evidence import MotionBranchResolution + + +def _solve(connection, request, diagnostic_path): + started, cpu = time.monotonic(), time.thread_time() + try: + result = solve_initialization(request) + if diagnostic_path is not None: + record_solver_event(diagnostic_path, request, "worker_solve_finished", + stamp_ns=time.time_ns(), monotonic_time=time.monotonic(), + elapsed_seconds=time.monotonic()-started, + thread_cpu_seconds=time.thread_time()-cpu, + resolved=result.resolved, reason=result.reason) + connection.send((result, None)) + except Exception: + connection.send((None, traceback.format_exc())) + finally: + connection.close() + + +class IsolatedBranchSolver: + """One bounded child per request; children have no SDK or ROS interfaces. + + Cancellation only stops computation. The coordinator's epoch and motion + checks remain the sole authority for accepting a returned resolution. + """ + + def __init__(self, diagnostic_path=None, *, timeout_seconds=120.): + self.diagnostic_path = diagnostic_path + self.timeout_seconds = timeout_seconds + self._closed = threading.Event() + + def __call__(self, request): + if self._closed.is_set(): + return MotionBranchResolution(False, "image_motion_solver_cancelled") + context = multiprocessing.get_context("spawn") + receive, send = context.Pipe(duplex=False) + process = context.Process(target=_solve, args=(send, request, self.diagnostic_path), daemon=True) + started = time.monotonic() + try: + process.start() + send.close() + while not self._closed.is_set(): + if receive.poll(.05): + result, error = receive.recv() + if error is not None: + raise RuntimeError(f"geometry worker failed:\n{error}") + return result + if time.monotonic()-started >= self.timeout_seconds: + return MotionBranchResolution(False, "image_motion_solver_timeout") + if not process.is_alive(): + raise RuntimeError(f"geometry worker exited without result: {process.exitcode}") + return MotionBranchResolution(False, "image_motion_solver_cancelled") + finally: + receive.close() + send.close() + if process.pid is not None: + if process.is_alive(): + process.terminate() + process.join() + process.close() + + def close(self): + self._closed.set() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py index 7a38a0c..cb0d6a3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -1,14 +1,28 @@ """Profile observation capture shared by every SDK and ROS camera host.""" from collections import deque -from dataclasses import dataclass +from dataclasses import asdict, dataclass +from threading import RLock import numpy as np from scipy.spatial.transform import Rotation from ..core.geometry.extrinsics import matrix_payload, transform_matrix from ..core.geometry.pnp import SquareTagPose, SquareTagPoseTracker +from ..core.geometry.tag_pose.fixed_reference import FixedTagReference +from ..core.geometry.image_motion_replay import has_image_model +from .pose_evidence import snapshot_pose_evidence from .scan_quality import observation_streams +from .branch_initialization import MotionBranchModel +from .branch_tracking import select_motion_poses + + +@dataclass(frozen=True) +class CaptureTiming: + received_stamp_ns: int + history_snapshot_stamp_ns: int + latest_feedback_stamp_ns: int | None + latest_command_stamp_ns: int | None @dataclass(frozen=True) @@ -21,31 +35,136 @@ class CaptureFrame: command: tuple[float, ...] | None skew_ns: int = 0 command_directions: tuple[str, ...] = () + observed_roles: frozenset[str] = frozenset() + timing: CaptureTiming | None = None + + +class _FramePoseEvidence: + """Keep each model's atomic current-frame metadata after parent-chain solves.""" + + def __init__(self, tracker): + self.tracker = tracker + self.by_role = {} + + def register(self, roles, snapshot): + self.by_role.update((role, snapshot) for role in roles) + + def observation_snapshot(self, role, stamp_ns): + return self.by_role.get(role, self.tracker).observation_snapshot(role, stamp_ns) class ObservationCapture: - def __init__(self, profile, *, reference_lock, extrinsics, tag_size_m=0.016, trackers=None): + def __init__(self, profile, *, reference_lock, extrinsics, tag_size_m=0.016, trackers=None, + projection_worker=None): self.profile, self.reference_lock, self.extrinsics = profile, reference_lock, extrinsics + self.projection_worker = projection_worker self.tag_size_m = float(tag_size_m) self.trackers = trackers or {view.name: SquareTagPoseTracker() for view in profile.vision.views} - self._seen = set() + self._metadata_lock = RLock() + self._metadata_generation = 0 + self._latest_stamp_by_view = {} self._poses = {} + self._pose_revisions = {} self.locked_poses = {} + self.locked_revisions = {} self.installations = {} + self._installation_revisions = {} self.last_missing = {} self.last_filtered = {} + self._branch_events_active = set() + self._motion_models = {} def reset(self): - self._seen.clear() - self._poses.clear() - self.locked_poses.clear() - self.installations.clear() - self.last_missing.clear() - self.last_filtered.clear() + with self._metadata_lock: + self._metadata_generation += 1 + self._latest_stamp_by_view.clear() + self._poses.clear() + self._pose_revisions.clear() + self.locked_poses.clear() + self.locked_revisions.clear() + self.installations.clear() + self._installation_revisions.clear() + self.last_missing.clear() + self.last_filtered.clear() + self._branch_events_active.clear() + self._motion_models.clear() for tracker in self.trackers.values(): tracker.reset() + def install_motion_model(self, model: MotionBranchModel): + """Add evidence before zero capture without replacing existing roles.""" + key = (model.task_name, model.view) + with self._metadata_lock: + owned = self._model_children(model) + for installed in self._motion_models.values(): + if installed.view != model.view: + continue + shared = owned & self._model_children(installed) + if shared and any(dict(installed.evidence_ids).get(role) != dict(model.evidence_ids).get(role) + for role in shared): + raise ValueError("motion_branch_evidence_replacement_forbidden") + if shared and installed.image_model != model.image_model: + raise ValueError("image_motion_model_replacement_forbidden") + previous = self._motion_models.get(key) + if previous is not None: + ids, new_ids = dict(previous.evidence_ids), dict(model.evidence_ids) + if any(ids[role] != new_ids[role] for role in ids.keys() & new_ids.keys()): + raise ValueError("motion_branch_evidence_replacement_forbidden") + old = {item.relation.joint: item for item in previous.constraints} + for item in model.constraints: + if item.relation.joint in old and old[item.relation.joint] != item: + raise ValueError("motion_constraint_replacement_forbidden") + old[item.relation.joint] = item + if (previous.image_model is not None and model.image_model is not None + and previous.image_model != model.image_model): + raise ValueError("image_motion_model_replacement_forbidden") + model = MotionBranchModel(model.task_name, model.view, + tuple(old.values()), tuple(sorted({**ids, **new_ids}.items())), + image_model=model.image_model or previous.image_model) + if previous != model: + self._metadata_generation += 1 + self._motion_models[key] = model + + @staticmethod + def _model_children(model): + geometry = model.constraints if model.image_model is None else model.image_model.geometry + return {item.relation.child_role for item in geometry} + + def _motion_model_schedule(self, view, motion): + """Keep installed identities while any task, return or idle pose changes. + + A model-authorized Tag can never return to independent IPPE selection. + Tracking all installed models also keeps continuity through clearance + and baseline returns; sample collection remains gated by the task below. + """ + del motion # Collection scope does not determine physical Tag identity. + available = {key: model for key, model in self._motion_models.items() if model.view == view} + if not available: + return () + owners = {role: key for key, model in available.items() for role in self._model_children(model)} + ordered, visiting, visited = [], set(), set() + + def include(key): + if key in visited: + return + if key in visiting: + raise ValueError("image_motion_model_dependency_cycle") + visiting.add(key) + model = available[key] + geometry = model.constraints if model.image_model is None else model.image_model.geometry + roots = {item.relation.parent_role for item in geometry} - self._model_children(model) + for role in sorted(roots): + if role in owners and owners[role] != key: + include(owners[role]) + visiting.remove(key) + visited.add(key) + ordered.append(model) + + for key in sorted(available): + include(key) + return tuple(ordered) + @staticmethod def _median_pose(poses): return SquareTagPose( @@ -53,63 +172,207 @@ class ObservationCapture: translation_xyz_m=tuple(np.median([p.translation_xyz_m for p in poses], axis=0)), reprojection_error_px=float(np.median([p.reprojection_error_px for p in poses]))) - def consume(self, frame: CaptureFrame, motion=None, *, collect_installations=False): - identity = (frame.view, frame.stamp_ns) - if frame.stamp_ns <= 0 or identity in self._seen: + @staticmethod + def _image_model_roles(roles, evidence): + """Include frozen moving parents' complete ancestral image evidence.""" + required, pending = set(roles), list(roles) + while pending: + item = evidence.get(pending.pop(), {}) + model = item.get("frozen_image_model") + if model is None: + continue + ancestors = {role for role, _ in model["tag_sizes"]} - required + required.update(ancestors) + pending.extend(ancestors) + return required + + def _candidate_frame(self, frame, motion, task, roles, evidence, selected, locked): + """Persist the exact decision alongside unchanged original candidates.""" + observations = {} + for role in sorted(roles): + if role not in frame.corners and role not in locked: + continue + item = dict(evidence[role]) + pose = selected.get(role) + item.update(locked_reference=role in locked, + selected=None if pose is None else { + "quaternion_xyzw": list(pose.quaternion_xyzw), + "translation_xyz_m": list(pose.translation_xyz_m), + "reprojection_error_px": pose.reprojection_error_px}, + maximum_reprojection_error_px=self.trackers[frame.view].maximum_reprojection_error_px) + item.pop("selected_pose", None) + observations[role] = item + return {"kind": "pnp_candidate_frame", "task_name": task.key, + "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + "view": frame.view, "image_stamp_ns": frame.stamp_ns, "tag_size_m": self.tag_size_m, + "sample_phase": motion.phase if motion.phase in {"steady", "joint_zero"} else "sweep", + "camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]", + "input_is_rectified": True, "roles": observations} + + def consume(self, frame: CaptureFrame, motion=None, *, collect_installations=False, + diagnostic=False, allow_recording=True, accepted_joints: frozenset[str] | None = None, + fixed_reference_only=False): + if fixed_reference_only and (motion is not None or collect_installations): + raise ValueError("fixed reference monitoring cannot acquire motion or installations") + if frame.view not in self.reference_lock.fixed_tag_by_view: return (), None - self._seen.add(identity) + with self._metadata_lock: + if frame.stamp_ns <= self._latest_stamp_by_view.get(frame.view, 0): + return (), None + self._latest_stamp_by_view[frame.view] = frame.stamp_ns + generation = self._metadata_generation + models = () if fixed_reference_only else self._motion_model_schedule(frame.view, motion) view = next(v for v in self.profile.vision.views if v.name == frame.view) + tags = tuple(tag for tag in view.tags if not fixed_reference_only or tag.fixed_reference) base = next(tag for tag in view.tags if tag.fixed_reference) - selected, filtered = {}, [] - for tag in view.tags: + cached = self.locked_reference(frame.view) + fixed_reference = None if cached is None else FixedTagReference(*cached) + selected, filtered, evidence, events, reasons = {}, [], {}, [], {} + evidence_tracker = _FramePoseEvidence(self.trackers[frame.view]) + managed_roles = {role for model in models for role in self._model_children(model)} + for tag in tags: points = frame.corners.get(tag.role) if points is None: continue pose, reason = self.trackers[frame.view].estimate(tag.role, points, - tag_size_m=tag.size_m, camera_matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns) + tag_size_m=tag.size_m, camera_matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns, + fixed_reference=fixed_reference if tag.fixed_reference else None, + **({"motion_managed": True} if tag.role in managed_roles else {})) + reasons[tag.role] = reason + if pose is not None: + selected[tag.role] = pose + for model in models: + poses, rejected, model_evidence = select_motion_poses(frame, model, self.trackers[frame.view], selected, + base=base, cached=cached, + **({"projection_worker": self.projection_worker} if self.projection_worker is not None else {})) + evidence_tracker.register(self._model_children(model), model_evidence) + for role in rejected: + selected.pop(role, None) + selected.update(poses) + reasons.update({role: "" for role in poses}) + reasons.update(rejected) + for tag in tags: + points = frame.corners.get(tag.role) + if points is None: + continue + pose, reason = selected.get(tag.role), reasons[tag.role] + item = snapshot_pose_evidence({tag.role: tag}, {tag.role: points}, + {} if pose is None else {tag.role: pose}, evidence_tracker, (), + stamp_ns=frame.stamp_ns)[tag.role] + diagnostics = item["candidate_diagnostics"] + if diagnostics.get("observation_stamp_ns") != frame.stamp_ns: + pose, reason = None, "pose_branch_observation_superseded" + elif pose is not None and diagnostics.get("branch_status") != "tracking": + pose, reason = None, "pose_branch_unconfirmed" + evidence[tag.role] = item + tracking_rejected = pose is None and ( + reason.startswith("pose_branch") or reason == "fixed_reference_pose_unverified") if pose is None: + selected.pop(tag.role, None) filtered.append(tag.tag_id) + if tracking_rejected: + events.append({"kind": "pnp_branch_event", "view": frame.view, + "image_stamp_ns": frame.stamp_ns, "tag_role": tag.role, + "tag_id": tag.tag_id, "reason": reason, + "task_name": None if motion is None else motion.task_key, + "motion_phase": None if motion is None else motion.phase, + "command_unit": self.profile.command.unit, + "command_vector": None if frame.command is None else list(frame.command), + "feedback_vector": None if frame.feedback is None else list(frame.feedback), + "state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6, + **({"timing": asdict(frame.timing)} if frame.timing is not None else {}), + "pnp_observation_evidence": item}) else: selected[tag.role] = pose - self.last_missing[frame.view] = tuple(tag.tag_id for tag in view.tags if tag.role not in frame.corners) - self.last_filtered[frame.view] = tuple(filtered) - movement = None - if frame.view in self.reference_lock.references and base.role in frame.corners: - # Once locked, fixed-object drift is a CORNER measurement. A large - # real displacement may fail the temporal PnP jump filter and must - # not consequently evade the ten-frame movement protection. - movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) - if base.role in selected: - # Only independent frames with a valid pose may establish a lock. - if frame.view not in self.reference_lock.references: - movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) - if self.reference_lock.locking_enabled and frame.view not in self.locked_poses: - buffer = self._poses.setdefault(frame.view, deque(maxlen=30)) - buffer.append(selected[base.role]) - if frame.view in self.reference_lock.references and len(buffer) >= self.reference_lock.minimum_frames: - self.locked_poses[frame.view] = self._median_pose(buffer) + with self._metadata_lock: + if (generation != self._metadata_generation + or self._latest_stamp_by_view.get(frame.view) != frame.stamp_ns): + return (), None + if pose is not None and (frame.view, tag.role) in self._branch_events_active: + events.append({"kind": "pnp_branch_event", "view": frame.view, + "image_stamp_ns": frame.stamp_ns, "tag_role": tag.role, + "tag_id": tag.tag_id, "reason": "pose_branch_recovered", + "pnp_observation_evidence": item}) + self._branch_events_active.discard((frame.view, tag.role)) + if tracking_rejected: + self._branch_events_active.add((frame.view, tag.role)) + with self._metadata_lock: + if (generation != self._metadata_generation + or self._latest_stamp_by_view.get(frame.view) != frame.stamp_ns): + return (), None + self.last_missing[frame.view] = tuple(tag.tag_id for tag in view.tags if tag.role not in frame.corners) + self.last_filtered[frame.view] = tuple(filtered) + movement = self._observe_fixed_reference(frame, base, selected, evidence) # Include the last locking frame exactly once; never update mounting # fingerprints during task motion or while the worker is fitting. if collect_installations and self.reference_lock.locking_enabled: - self._observe_installations(frame.view, base.role, selected, + self._observe_installations(frame.view, base.role, selected, evidence, prefix="" if collect_installations is True else str(collect_installations)+"/") - if movement is not None or motion is None or not motion.recording: - return (), movement + if motion is not None and motion.phase == "zero_approach" and motion.reference_joints: + events.append({"kind": "motion_branch_observation", "view": frame.view, + "image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key, + "zero_joints": list(motion.reference_joints), "sample_phase": "zero_approach", + "command_unit": self.profile.command.unit, + "command_vector": None if frame.command is None else list(frame.command), + "feedback_vector": None if frame.feedback is None else list(frame.feedback), + "state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6, + "camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True, + "tags": evidence, "is_accepted_joint_sample": False}) + diagnostic_raw = (diagnostic and motion is not None and ( + motion.phase == "steady" or (motion.phase in {"sweep", "joint_zero"} and allow_recording))) + if diagnostic_raw: + # Raw candidate capture has its own record type. It cannot be + # mistaken for an accepted joint angle or enter formal fitting. + events.append({"kind": "diagnostic_observation_frame", "view": frame.view, + "image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key, + "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + "sample_phase": motion.phase, "command_unit": self.profile.command.unit, + **({"zero_joints": list(motion.zero_joints)} if motion.phase == "joint_zero" else {}), + **({"steady_index": motion.steady_index, + "steady_target": None if motion.command_index is None else motion.target[motion.command_index], + "minimum_hold_seconds": motion.minimum_hold_seconds, + "target_command_vector": list(motion.target), + "accepted_sample_window_open": bool(allow_recording)} if motion.phase == "steady" else {}), + "command_vector": None if frame.command is None else list(frame.command), + "feedback_vector": None if frame.feedback is None else list(frame.feedback), + "state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6, + **({"timing": asdict(frame.timing)} if frame.timing is not None else {}), + "camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True, + "tags": evidence, "is_accepted_joint_sample": False}) + if movement is not None or motion is None or not motion.recording or not allow_recording: + return tuple(events), movement task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key) streams = [item for item in observation_streams(self.profile, task) if item[2].view == frame.view] if motion.phase == "joint_zero": streams = [item for item in streams if item[1] in motion.zero_joints] + if diagnostic: + streams = [item for item in streams if item[1] in (accepted_joints or ())] if not streams or frame.feedback is None or frame.command is None: - return (), movement - locked = [] - if base.role not in selected and frame.view in self.locked_poses: - selected[base.role] = self.locked_poses[frame.view] + return tuple(events), movement + locked = [role for role, item in evidence.items() + if role in selected and item["pose_source"] == "verified_fixed_reference"] + cached = self.locked_reference(frame.view) + if (base.role not in frame.corners and base.role not in frame.observed_roles + and cached is not None + and self.trackers[frame.view].freeze_roles({base.role: cached[1]})): + selected[base.role] = cached[0] locked.append(base.role) + evidence.update(snapshot_pose_evidence({base.role: base}, {}, + {base.role: selected[base.role]}, self.trackers[frame.view], locked, + stamp_ns=frame.stamp_ns, + reference_revisions={base.role: cached[1]})) roles = {role for _, _, spec in streams for role in (spec.parent_role, spec.child_role)} + image_geometry = any(has_image_model(evidence.get(role, {})) for role in roles) + if image_geometry: + roles = self._image_model_roles(roles, evidence) + if any(role not in selected or role not in frame.corners for role in roles): + # Every ancestor is verified in this image. A partial chain or + # cached root cannot become an accepted constrained observation. + return tuple(events), movement if task.key in self.profile.measurement.candidate_selection_tasks and motion.phase != "joint_zero": if any(role not in selected or (role not in frame.corners and role not in locked) for role in roles): - return (), movement # An incomplete articulated frame is a discarded sample. - rows = [] + return tuple(events), movement # An incomplete articulated frame is a discarded sample. + rows = list(events) common_from_view = self.extrinsics.transform(frame.view) unit = self.profile.command.unit for field, joint, spec in streams: @@ -136,10 +399,15 @@ class ObservationCapture: "view_normal_common_xyz": common_from_view[:3, 2].tolist(), "camera_center_common_xyz_m": common_from_view[:3, 3].tolist(), "locked_reference_roles": locked, - "pnp_reprojection_error_px": max(parent.reprojection_error_px, child.reprojection_error_px)} + "pnp_reprojection_error_px": max(parent.reprojection_error_px, child.reprojection_error_px), + "pnp_observation_evidence": {"parent": evidence[spec.parent_role], + "child": evidence[spec.child_role]}} if motion.phase == "steady": row.update(steady_index=motion.steady_index, steady_target=motion.target[task.command_index]) + if motion.sampling_plan_sha256: + row.update(steady_training_nodes=list(motion.steady_training_nodes), + sampling_plan_sha256=motion.sampling_plan_sha256) if spec.parent_role in frame.corners and spec.child_role in frame.corners: row["image_relative_xy_px"] = (np.mean(frame.corners[spec.child_role], axis=0) - np.mean(frame.corners[spec.parent_role], axis=0)).tolist() @@ -149,39 +417,94 @@ class ObservationCapture: if motion.phase == "joint_zero": row["kind"] = "joint_zero_sample" row["joint"] = joint - if task.key in self.profile.measurement.candidate_selection_tasks: - rows.append({"kind": "pnp_candidate_frame", "task_name": task.key, - "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, - "view": frame.view, "image_stamp_ns": frame.stamp_ns, "tag_size_m": self.tag_size_m, - "camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]", - "input_is_rectified": True, - "roles": {role: {"corners_xy": np.asarray(frame.corners[role]).tolist() if role in frame.corners else None, - "locked_reference": role in locked, - "tag_size_m": next(tag.size_m for tag in view.tags if tag.role == role), - "selected": None if role not in selected else { - "quaternion_xyzw": list(selected[role].quaternion_xyzw), - "translation_xyz_m": list(selected[role].translation_xyz_m)}, - "maximum_reprojection_error_px": self.trackers[frame.view].maximum_reprojection_error_px} - for role in roles if role in frame.corners or role in locked}}) + if ((task.key in self.profile.measurement.candidate_selection_tasks or image_geometry) + and len(rows) > len(events)): + rows.append(self._candidate_frame(frame, motion, task, roles, evidence, selected, locked)) return tuple(rows), movement - def _observe_installations(self, view, base_role, poses, *, prefix=""): + def _observe_fixed_reference(self, frame, base, selected, evidence): + movement = None + if frame.view not in self.reference_lock.missing_views and base.role in frame.corners: + # Pose rejection must not mask actual image-space movement. + movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) + if base.role not in selected: + return movement + if frame.view in self.reference_lock.missing_views: + movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) + if not self.reference_lock.locking_enabled: + return movement + revision = evidence[base.role]["candidate_diagnostics"]["branch_revision"] + with self._metadata_lock: + if frame.view in self.locked_poses: + return movement + generation = self._metadata_generation + buffer = self._poses.setdefault(frame.view, deque(maxlen=30)) + if self._pose_revisions.get(frame.view) != revision: + buffer.clear() + self._pose_revisions[frame.view] = revision + buffer.append(selected[base.role]) + poses = tuple(buffer) + if frame.view in self.reference_lock.missing_views or len(poses) < self.reference_lock.minimum_frames: + return movement + median = self._median_pose(poses) + with self._metadata_lock: + if (generation == self._metadata_generation and frame.view not in self.locked_poses + and self._pose_revisions.get(frame.view) == revision + and self.trackers[frame.view].freeze_roles({base.role: revision})): + self.locked_poses[frame.view] = median + self.locked_revisions[frame.view] = revision + return movement + + def _observe_installations(self, view, base_role, poses, evidence, *, prefix=""): if base_role not in poses: return + with self._metadata_lock: + generation = self._metadata_generation parent = poses[base_role] p = transform_matrix(parent.translation_xyz_m, parent.quaternion_xyzw) + observations = [] for role, pose in poses.items(): if role == base_role: continue + key = f"{prefix}{view}:{role}" + revisions = tuple(evidence[name]["candidate_diagnostics"]["branch_revision"] + for name in (base_role, role)) relative = np.linalg.inv(p) @ transform_matrix(pose.translation_xyz_m, pose.quaternion_xyzw) - buffer = self.installations.setdefault(f"{prefix}{view}:{role}", deque(maxlen=30)) - buffer.append(SquareTagPose( + observations.append((key, revisions, SquareTagPose( quaternion_xyzw=tuple(Rotation.from_matrix(relative[:3, :3]).as_quat()), - translation_xyz_m=tuple(relative[:3, 3]), reprojection_error_px=pose.reprojection_error_px)) + translation_xyz_m=tuple(relative[:3, 3]), reprojection_error_px=pose.reprojection_error_px))) + with self._metadata_lock: + if generation != self._metadata_generation: + return + for key, revisions, pose in observations: + buffer = self.installations.setdefault(key, deque(maxlen=30)) + if self._installation_revisions.get(key) != revisions: + buffer.clear() + self._installation_revisions[key] = revisions + buffer.append(pose) + + def locked_reference(self, view): + with self._metadata_lock: + if view not in self.locked_poses: + return None + return self.locked_poses[view], self.locked_revisions[view] + + def locked_reference_views(self): + with self._metadata_lock: + return frozenset(self.locked_poses) + + def installation_count(self, key): + with self._metadata_lock: + return len(self.installations.get(key, ())) def fingerprint_poses(self): + # Copy both the dictionary and each deque while writers hold the same + # short lock. Rotation means and NumPy work use only immutable snapshots. + with self._metadata_lock: + fixed = tuple(self.locked_poses.items()) + moving = tuple((name, tuple(rows)) for name, rows in self.installations.items()) def payload(pose): return {"rotation_xyzw": list(pose.quaternion_xyzw), "translation_xyz_m": list(pose.translation_xyz_m)} - return ({view: payload(pose) for view, pose in self.locked_poses.items()}, - {name: payload(self._median_pose(rows)) for name, rows in self.installations.items() + return ({view: payload(pose) for view, pose in fixed}, + {name: payload(self._median_pose(rows)) for name, rows in moving if len(rows) >= self.reference_lock.minimum_frames}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index 1d6bb84..4a11122 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -19,16 +19,21 @@ import numpy as np from ..acquisition import StateSample, interpolate_state_u8 from ..core.domain.profile import CalibrationProfile from ..core.domain.reference import build_joint_zero_reference -from ..storage import append_jsonl, atomic_write_json -from .engine import ACQUISITION_POLICY_VERSION +from ..core.geometry.pnp import POSE_TRACKING_POLICY_VERSION, SquareTagPoseTracker +from ..storage import append_jsonl, append_jsonl_many, atomic_write_json, sync_jsonl +from .engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION from .reference_lock import ReferenceLock -from .acquisition import load_capture from .artifacts.controller import FinalizationController, FinalizationInputs, FinalizationProtocolError from .cameras import CameraObservations from .camera_preflight import CameraPreflight -from .capture import CaptureFrame, ObservationCapture +from .capture import CaptureFrame, CaptureTiming, ObservationCapture +from .branch_initialization import BranchInitialization, solve_initialization +from .branch_diagnostics import record_solver_event from .execution import SessionExecution +from .diagnostic_capture import diagnostic_hold_timing, resolve_diagnostic_capture, reject_diagnostic_capture +from .diagnostic_reference import build_diagnostic_zero_attempt from .inputs import AdapterFactory, CameraModelInput, DetectionInput, RuntimePorts, ServiceResult +from .pose_evidence import reference_branch_revisions from .motion_execution import MotionExecution from .parameters import RuntimeParameters from .resume import ResumeDecision, ResumeVerifier, fingerprint_from_mapping @@ -37,10 +42,16 @@ from .session import CalibrationPhase as Phase from .snapshot import DeviceReadiness, CameraReadiness, RuntimeSnapshot, build_snapshot, legacy_state +_TERMINAL_STATES = frozenset({"PASSED", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"}) + + class CalibrationCoordinator: def __init__(self, profile: CalibrationProfile, parameters: RuntimeParameters, - ports: RuntimePorts, adapter_factory: AdapterFactory, *, finalization=None): + ports: RuntimePorts, adapter_factory: AdapterFactory, *, finalization=None, + initialization_solver=None, projection_worker=None): self.profile, self.parameters, self.ports = profile, parameters, ports + self.initialization_solver = initialization_solver + self.projection_worker = projection_worker self.step_data_lock = threading.RLock() self.model_name = profile.key.model self.command_names = tuple(profile.command.names) @@ -48,12 +59,17 @@ class CalibrationCoordinator: self.command_unit = profile.command.unit self.feedback_lower = tuple(profile.command.minimum_feedback_values) self.feedback_upper = tuple(profile.command.maximum_feedback_values) - self.execution = SessionExecution(profile) + self.diagnostic_capture = resolve_diagnostic_capture(profile, parameters.diagnostic_capture) + if self.diagnostic_capture is not None and ( + not parameters.commands_enabled or parameters.resume_raw_samples_path is not None): + raise ValueError("diagnostic_capture_requires_fresh_command_enabled_session") + self.execution = SessionExecution(profile, diagnostic_capture=self.diagnostic_capture) self.safety = SafetyPolicy(profile) self.reference_lock = self._new_reference_lock() - self.trackers = parameters.new_trackers(profile.vision.view_names) + self.trackers = parameters.new_trackers(self.required_observation_views) self.capture = ObservationCapture(profile, reference_lock=self.reference_lock, - extrinsics=parameters.extrinsics, tag_size_m=parameters.tag_size_m, trackers=self.trackers) + extrinsics=parameters.extrinsics, tag_size_m=parameters.tag_size_m, trackers=self.trackers, + projection_worker=projection_worker) self.latest_feedback: tuple[float, ...] = () self.state_history: deque[StateSample] = deque(maxlen=2000) self.state_receive_times: deque[float] = deque(maxlen=300) @@ -61,6 +77,7 @@ class CalibrationCoordinator: self.command_direction_history = deque(maxlen=2000) self._command_directions = [""]*self.command_count self.raw_records = [] + self._image_record_stamps = {} self.last_command = None self.commanded_speed = None self.step_speed_ready_at = 0.0 @@ -70,17 +87,34 @@ class CalibrationCoordinator: self._observation_epoch = 0 self._pause_effects_applied = False self.segment = None + self._settled_pose = None self._segment_number = 0 self._motion = None + self._motion_started_ns = 0 self._steady_capture_after_ns = None - self._resume_message = "not_requested" + self._zero_geometry_started_at = None + self._zero_geometry_completed_at = None + self._zero_capture_started_at = None + self._resume_message = "diagnostic_capture_no_resume" if self.diagnostic_capture else "not_requested" self._resumed_count = 0 - from .joint_resume import JointResume + self._resume_import = None + from .joint_resume import JointResume, PreparedResume self.joint_resume = JointResume(profile) + self._resume_checkpoint, self._resume_checkpoint_error = None, "" + if parameters.resume_raw_samples_path is not None: + try: + self._resume_checkpoint = PreparedResume.load(profile, self.execution.session.engine, + parameters.resume_raw_samples_path, parameters.serial_number) + except (OSError, ValueError, KeyError, TypeError) as error: + self._resume_checkpoint_error = str(error) self._unit_rows = [] self._capture_unit_key = None self._steady_rows = [] self._zero_reference_error = "" + self._branch_observation_reasons = {} + self.branch_initialization = BranchInitialization(profile, source_urdf=parameters.source_urdf) + from .zero_recovery import ZeroRecovery + self.zero_recovery = ZeroRecovery() self._reference_capture_key = None self._reference_observation_started = None self._current_fingerprint = {} @@ -94,14 +128,23 @@ class CalibrationCoordinator: validate_executable_profile(profile, parameters.source_urdf) parameters.session_dir.mkdir(parents=True, exist_ok=True) self.raw_path = parameters.session_dir / "raw_samples.jsonl" - append_jsonl(self.raw_path, { + self._session_header = { "kind": "session_start", "sample_schema_version": profile.artifacts.output_schema_version, "profile_id": profile.key.profile_id, "serial_number": parameters.serial_number, "curve_input_domain": profile.curve_input_domain, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": self.execution.session.engine.capture_schedule_version, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, + "required_observation_views": list(self.required_observation_views), **parameters.protected_inputs, "resume_checkpoint_requested": parameters.resume_raw_samples_path is not None, - }) + **({"diagnostic_capture": self.diagnostic_capture.as_dict()} if self.diagnostic_capture else {}), + } + if profile.acquisition.command_capture_mode == "separate" and self.diagnostic_capture is None: + from ..core.fitting.command_sampling import SAMPLING_POLICY + self._session_header["command_sampling_policy"] = SAMPLING_POLICY + append_jsonl(self.raw_path, self._session_header) + self.raw_records.append(self._session_header) self.cameras = CameraObservations(parameters.extrinsics, self.raw_path, ports.monotonic) self.sdk_adapter = adapter_factory(self._write_position, self._write_speed, self.feedback_fresh) self.finalization = finalization or FinalizationController(profile, parameters.session_dir) @@ -114,6 +157,10 @@ class CalibrationCoordinator: def started(self) -> bool: return self._started_at is not None + @property + def required_observation_views(self) -> tuple[str, ...]: + return self.execution.session.required_observation_views + def start(self) -> ServiceResult: with self.step_data_lock: if self.execution.session.phase != Phase.READY or not self.parameters.commands_enabled: @@ -128,14 +175,20 @@ class CalibrationCoordinator: # may finish later, but cannot populate the new formal reference. self.reference_lock = self._new_reference_lock() self.reference_lock.begin_session() - self.trackers = self.parameters.new_trackers(self.profile.vision.view_names) + self.trackers = self.parameters.new_trackers(self.required_observation_views) self.capture = ObservationCapture(self.profile, reference_lock=self.reference_lock, - extrinsics=self.parameters.extrinsics, tag_size_m=self.parameters.tag_size_m, trackers=self.trackers) + extrinsics=self.parameters.extrinsics, tag_size_m=self.parameters.tag_size_m, trackers=self.trackers, + projection_worker=self.projection_worker) self.state_history.clear() self.command_history.clear() self.command_direction_history.clear() self._command_directions = [""]*self.command_count - self.raw_records.clear() + # Both the in-memory and process finalizers consume a self-contained + # capture, including the source identity used by CAD constraints. + self.raw_records[:] = [self._session_header] + self._image_record_stamps.clear() + self._branch_observation_reasons.clear() + self.branch_initialization.clear() self.execution.session.start(resume_requested=self.parameters.resume_raw_samples_path is not None) self._started_at = self.ports.monotonic() self._observation_epoch += 1 @@ -145,6 +198,10 @@ class CalibrationCoordinator: def abort(self) -> ServiceResult: with self.step_data_lock: + if self.state in {"FAILED", "ABORTED"}: + return ServiceResult(False, self.reason) + if self.state == "DIAGNOSTIC_COMPLETE": + return ServiceResult(False, "diagnostic_capture_already_complete") if self.state == "PASSED": return ServiceResult(False, "calibration_already_published") self._pause("operator_abort_holding_current_position") @@ -152,22 +209,42 @@ class CalibrationCoordinator: self.reason = "operator_abort_holding_current_position" return ServiceResult(True, self.reason) - def receive_feedback(self, names, positions, stamp_ns: int) -> None: + def receive_feedback(self, names, positions, stamp_ns: int, *, channel_stamps_ns=()) -> StateSample | None: with self.step_data_lock: - self._accept_feedback(names, positions, stamp_ns) + return self._accept_feedback(names, positions, stamp_ns, channel_stamps_ns=channel_stamps_ns) def receive_camera_info(self, view: str, message: CameraModelInput) -> None: with self.step_data_lock: + if (view not in self.required_observation_views or self.finalization.started + or self.state in _TERMINAL_STATES): + return self.cameras.accept(view, message, started=self.started, pause=self._pause, trackers=self.trackers) + def _record_solver_event(self, request, event, **details): + record_solver_event(self.parameters.session_dir / "motion_solver_diagnostic.jsonl", + request, event, stamp_ns=self.ports.clock_ns(), + monotonic_time=self.ports.monotonic(), **details) + def receive_detections(self, observation: DetectionInput) -> None: view, stamp = observation.view, observation.stamp_ns + received_stamp = self.ports.clock_ns() with self.step_data_lock: + if (view not in self.required_observation_views or self.finalization.started + or self.state in _TERMINAL_STATES): + return self.cameras.detections_received_at[view] = self.ports.monotonic() - if view not in self.cameras.matrices or self.state in {"PASSED", "PAUSED", "ABORTED", "FAILED"}: + if view not in self.cameras.matrices or self.state in _TERMINAL_STATES: return capture, motion = self.capture, self._motion + if motion is not None and stamp < self._motion_started_ns: + return epoch = self._observation_epoch + motion_version = self._segment_number + initialization = self.branch_initialization.request(view, motion, + session_epoch=epoch, motion_version=motion_version) + if initialization is not None and self._zero_geometry_started_at is None: + self._zero_geometry_started_at = self.ports.monotonic() + accepted_joints = self._diagnostic_accepted_joints(motion) steady_after = self._steady_capture_after_ns locking = self._reference_capture_key or self.execution.session.phase == Phase.REFERENCE_LOCKING matrix = self.cameras.matrices[view].copy() @@ -175,10 +252,68 @@ class CalibrationCoordinator: feedback_history = tuple(self.state_history) command_history = tuple(self.command_history) direction_history = tuple(self.command_direction_history) + timing = CaptureTiming(received_stamp, self.ports.clock_ns(), + feedback_history[-1].stamp_ns if feedback_history else None, + command_history[-1].stamp_ns if command_history else None) + if initialization is not None: + solver_started = self.ports.monotonic() + self._record_solver_event(initialization, "request_created", + current_motion_version=motion_version) + try: + resolution = (self.initialization_solver or solve_initialization)(initialization) + except Exception: + self._record_solver_event(initialization, "solve_exception", + elapsed_seconds=self.ports.monotonic()-solver_started, + traceback=traceback.format_exc()) + raise + self._record_solver_event(initialization, "solve_finished", + elapsed_seconds=self.ports.monotonic()-solver_started, + resolved=resolution.resolved, reason=resolution.reason) + with self.step_data_lock: + if (epoch != self._observation_epoch or capture is not self.capture + or motion is not self._motion or motion_version != self._segment_number): + self._record_solver_event(initialization, "commit_rejected", + current_session_epoch=self._observation_epoch, + expected_current_motion_version=motion_version, + current_motion_version=self._segment_number, + capture_changed=capture is not self.capture, + motion_changed=motion is not self._motion) + return + accepted = self.branch_initialization.accept(initialization, resolution) + if accepted is None: + self._record_solver_event(initialization, "commit_rejected", + current_generation=self.branch_initialization.generation) + return + model, report = accepted + append_jsonl(self.raw_path, report) + self.raw_records.append(report) + if not self.branch_initialization.pending(): + self._zero_geometry_completed_at = self.ports.monotonic() + if model is not None: + capture.install_motion_model(model) + # Keep feedback readiness independent of geometry. If a + # stable window already exists, only post-confirmation + # images may establish zero; older callbacks are invalid. + self._steady_rows.clear() + self._zero_capture_started_at = None + if self._steady_capture_after_ns is not None: + self._steady_capture_after_ns = self.ports.clock_ns() + steady_after = self._steady_capture_after_ns + accepted_joints = self._diagnostic_accepted_joints(motion) + self._record_solver_event(initialization, "commit_accepted", + resolved=resolution.resolved) roles = {tag.tag_id: tag.role for tag in self._view(view).tags} - corners = {} + corners, quality_events = {}, [] + observed_roles = frozenset(roles[item.tag_id] for item in observation.detections if item.tag_id in roles) for detection in observation.detections: points = np.asarray(detection.corners, dtype=float) + if detection.border_quality is not None and not detection.border_quality["passed"]: + quality_events.append({"kind": "detection_quality_rejected", "view": view, + "image_stamp_ns": stamp, "tag_id": detection.tag_id, + "corners_xy": points.tolist(), "quality": dict(detection.border_quality), + "task_name": None if motion is None else motion.task_key, + "motion_phase": None if motion is None else motion.phase}) + continue if (detection.tag_id not in roles or points.shape != (4, 2) or not np.all(np.isfinite(points)) or detection.hamming > self.parameters.detection.maximum_hamming or detection.decision_margin < self.parameters.detection.minimum_decision_margin or np.min(np.linalg.norm(points-np.roll(points, -1, axis=0), axis=1)) < self.parameters.detection.minimum_edge_pixels @@ -192,29 +327,133 @@ class CalibrationCoordinator: if row.stamp_ns <= stamp and stamp-row.stamp_ns <= self.parameters.maximum_state_image_skew_ns), None) frame = CaptureFrame(view, stamp, matrix, corners, None if matched is None else matched[0], command, 0 if matched is None else matched[1], - next((d for t, d in reversed(direction_history) if t <= stamp), ())) + next((d for t, d in reversed(direction_history) if t <= stamp), ()), observed_roles, timing) record_motion = motion if motion is not None and motion.phase in {"steady", "joint_zero"} and (steady_after is None or stamp < steady_after): record_motion = None + from .image_capture import image_observation_record + raw_image = image_observation_record(self.profile, frame, record_motion) # Pose estimation is deliberately outside the state lock. Start swaps # capture objects; pause/abort invalidates the epoch; movement changes # the motion identity. None of their late results may enter the session. - rows, movement = capture.consume(frame, record_motion, collect_installations=locking) + rows, movement = capture.consume(frame, motion, collect_installations=locking, + diagnostic=self.diagnostic_capture is not None, allow_recording=record_motion is not None, + accepted_joints=accepted_joints) + rows = (*(() if raw_image is None else (raw_image,)), *quality_events, *rows) with self.step_data_lock: if (epoch != self._observation_epoch or capture is not self.capture - or motion is not self._motion): + or motion is not self._motion or motion_version != self._segment_number + or stamp <= self._image_record_stamps.get(view, 0)): return if motion is not None and motion.phase in {"steady", "joint_zero"} and steady_after != self._steady_capture_after_ns: - return + if self.diagnostic_capture is None or motion.phase != "steady" or motion.minimum_hold_seconds <= 0: + return + # The diagnostic needs the actual approach and recovery, too. + # Its unaccepted raw images are retained while ordinary joint + # samples still require an unchanged stable-feedback window. + rows = tuple(row for row in rows if row.get("kind") in { + "diagnostic_observation_frame", "pnp_branch_event"}) + record_motion = None if movement is not None: self._pause(f"fixed_reference_moved:{view}:{movement.drift_px}") return - for row in rows: + branch_events = [row for row in rows if row.get("kind") == "pnp_branch_event"] + self.raw_records.extend(branch_events) + # Per-record fsync while holding this lock stalls SDK feedback + # and command ticks. Persist references and scan checkpoints; + # flush continuous image evidence in batches between them. + append_jsonl_many(self.raw_path, branch_events, durable=False) + for row in branch_events: + if self._accept_branch_event(row): + return + sample_rows = [row for row in rows if row.get("kind") != "pnp_branch_event" + and (record_motion is not None + or (self.diagnostic_capture is not None and motion is not None + and motion.phase == "steady" and motion.minimum_hold_seconds > 0 + and row.get("kind") == "diagnostic_observation_frame") + or row.get("kind") not in { + "joint_zero_sample", "joint_sample", "secondary_joint_sample", "pnp_candidate_frame", "image_observation_frame", + "diagnostic_observation_frame"})] + for row in sample_rows: + if row.get("kind") == "image_observation_frame": + self._image_record_stamps[view] = stamp + row.update(session_epoch=epoch, motion_version=motion_version, + motion_started_ns=self._motion_started_ns) + if row.get("kind") == "diagnostic_observation_frame": + row.update(session_epoch=epoch, motion_version=motion_version, + motion_started_ns=self._motion_started_ns) + if motion.phase == "steady" and motion.minimum_hold_seconds > 0: + row.update(diagnostic_hold_timing(command_history, target=motion.target, + image_stamp_ns=stamp, session_epoch=epoch, motion_version=motion_version, + motion_started_ns=self._motion_started_ns, + feedback_stable_since_ns=steady_after)) + row["accepted_sample_window_open"] = record_motion is not None + if row.get("kind") == "motion_branch_observation": + row.update(session_epoch=epoch, motion_version=motion_version) + self.branch_initialization.observe(row) + self.raw_records.append(row) + continue + for item in row.get("pnp_observation_evidence", {}).values(): + diagnostics = item["candidate_diagnostics"] + if (diagnostics.get("observation_stamp_ns") == row.get("image_stamp_ns") + and diagnostics.get("branch_status") == "tracking"): + self._branch_observation_reasons.pop((row["view"], item["tag_role"]), None) self.raw_records.append(row) self._unit_rows.append(row) if motion is not None and motion.phase in {"steady", "joint_zero"}: self._steady_rows.append(row) - append_jsonl(self.raw_path, row) + if row.get("kind") == "joint_zero_sample": + row["capture_timing"] = { + "received_stamp_ns": received_stamp, + "committed_stamp_ns": self.ports.clock_ns(), + "history_snapshot_stamp_ns": timing.history_snapshot_stamp_ns, + } + if row.get("kind") == "joint_zero_sample" and self._zero_capture_started_at is None: + self._zero_capture_started_at = self.ports.monotonic() + append_jsonl_many(self.raw_path, sample_rows, durable=False) + + def _diagnostic_accepted_joints(self, motion): + """Immutable sample permissions, captured under the observation lock.""" + if self.diagnostic_capture is None: + return None + if motion is not None and motion.phase == "joint_zero": + return frozenset(motion.zero_joints) if self.branch_initialization.ready(motion) else frozenset() + return frozenset(self.execution.zero_references) + + def _accept_branch_event(self, row): + """Apply captured diagnostics only after the ordinary observation version gate.""" + if row["view"] not in self.required_observation_views: + return False + diagnostics = row["pnp_observation_evidence"]["candidate_diagnostics"] + if diagnostics.get("observation_stamp_ns") != row["image_stamp_ns"]: + return False + key = (row["view"], row["tag_role"]) + if diagnostics.get("branch_status") == "tracking": + self._branch_observation_reasons.pop(key, None) + return False + self._branch_observation_reasons[key] = (row["image_stamp_ns"], row["reason"]) + tag = next((tag for tag in self._view(row["view"]).tags + if tag.tag_id == row["tag_id"] and tag.role == row["tag_role"]), None) + if tag is None or row["reason"] != "pose_branch_conflict": + return False + required = tag.fixed_reference + unit = self.execution.session.current_unit + if unit is not None: + from .scan_quality import observation_streams + task = next(task for task in self.profile.motion.tasks if task.key == unit.task_key) + required = required or any(spec.view == row["view"] + and tag.role in {spec.parent_role, spec.child_role} + and (self._motion is None or self._motion.phase != "joint_zero" or name in self._motion.zero_joints) + for _, name, spec in observation_streams(self.profile, task)) + if required: + reason = f"{row['reason']}:view={row['view']}:tag={tag.tag_id}:role={tag.role}" + if tag.fixed_reference and diagnostics.get("pose_constraint") == "fixed_reference": + error = diagnostics.get("fixed_reference_reprojection_error_px", "unavailable") + reason = (f"fixed_reference_pose_inconsistent:view={row['view']}:tag={tag.tag_id}:" + f"role={tag.role}:reprojection_error_px={error}") + self._pause(reason) + return True + return False def tick(self) -> None: with self.step_data_lock: @@ -231,10 +470,14 @@ class CalibrationCoordinator: def close(self) -> None: with self.step_data_lock: self._observation_epoch += 1 - self.finalization.close() + self.branch_initialization.clear() + try: + sync_jsonl(self.raw_path) + finally: + self.finalization.close() def _advance(self): - if self.state in {"PASSED", "PAUSED", "ABORTED", "FAILED"}: + if self.state in _TERMINAL_STATES: return now = self.ports.monotonic() session = self.execution.session @@ -272,22 +515,24 @@ class CalibrationCoordinator: self._poll_finalization() return phase = session.phase + if self._resume_import is not None: + self._publish_command(list(current)) + self._import_resume_batch() + return if phase in {Phase.PREPARE, Phase.MAPPING_PROBE} and self.segment is None and session.current_unit is not None: used, imported = self.joint_resume.take_task(session.current_unit.task_key) if used: - self.execution.first_cycle_spans.update(self.joint_resume.spans_for_task(session.current_unit.task_key)) - for row in imported: - self.raw_records.append(row) - append_jsonl(self.raw_path, row) - self.execution.authorize_task_reuse(used) - self._resumed_count += len(used) + from .joint_resume import TaskResumeImport + self._resume_import = TaskResumeImport(session.current_unit.task_key, used, imported) + self._publish_command(list(current)) + self._import_resume_batch() return if phase == Phase.REFERENCE_LOCKING: self._publish_command(list(current)) if not self.reference_lock.locking_enabled: self.reference_lock.start_locking() self.reason = "locking_fixed_base_references:missing="+",".join(self.reference_lock.missing_views) - if self.reference_lock.locked and set(self.capture.locked_poses) == set(self.profile.vision.view_names): + if self.reference_lock.locked and self.capture.locked_reference_views() == set(self.required_observation_views): session.reference_locked() if session.phase != Phase.REFERENCE_POSES: self._save_reference() @@ -298,17 +543,32 @@ class CalibrationCoordinator: if phase == Phase.EVALUATE: action = self.execution.action quality = self.execution.evaluate(self.raw_records) - append_jsonl(self.raw_path, {"kind": "scan_unit_complete", "task_name": action.scan_unit.task_key, + record = {"kind": "scan_unit_complete", "task_name": action.scan_unit.task_key, "cycle": action.scan_unit.cycle, "direction": action.scan_unit.direction, "attempt": action.retry+1, "passed": quality.passed, - "failures": list(quality.failures), "metrics": dict(quality.metrics)}) + "failures": list(quality.failures), "metrics": dict(quality.metrics), + **({"quality_scope": "diagnostic_observation_coverage", + "accepted_pose_quality": self.execution.last_diagnostic_quality.accepted_pose_report(), + "observation_timing": dict(self.execution.last_diagnostic_quality.observation_timing)} + if self.execution.last_diagnostic_quality is not None else {})} + self.raw_records.append(record) + append_jsonl(self.raw_path, record) if session.phase == Phase.PAUSED: - self._pause("sweep_quality_failed:"+",".join(quality.failures)) + if "observation_timing" in record: + pause = session.status().pause + session.pause(pause.code, pause.reason, + {**pause.details, "observation_timing": record["observation_timing"]}, + suggestion=pause.suggestion) + task = next(task for task in self.profile.motion.tasks if task.key == action.scan_unit.task_key) + self._pause("sweep_quality_failed:"+",".join(quality.failures)+self._task_branch_details(task)) return if phase == Phase.FIT: self._motion = None self._finalize() return + if (phase == Phase.PREPARE and session.current_unit is not None + and session.current_unit.sample_phase == "steady" and self.diagnostic_capture is None): + self._prepare_command_sampling(session.current_unit.task_key) motion = self.execution.motion(current) if motion is None: if phase == Phase.PREPARE and session.current_unit is None: @@ -316,6 +576,8 @@ class CalibrationCoordinator: return self._pause(f"unhandled_session_phase:{phase.value}") return + if self.segment is None and self._reuse_settled_preparation(motion, now): + return if self.segment is None: if self.commanded_speed != motion.speed: self._publish_speed(motion.speed) @@ -325,7 +587,11 @@ class CalibrationCoordinator: return self._segment_number += 1 self._motion = motion + self._motion_started_ns = self.ports.clock_ns() self._steady_capture_after_ns = None + self._zero_geometry_started_at = None + self._zero_geometry_completed_at = None + self._zero_capture_started_at = None self._reference_capture_key = None self._reference_observation_started = None self._begin_motion_capture(motion) @@ -333,7 +599,7 @@ class CalibrationCoordinator: initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number)) shaped = self.segment.sample(now) self._publish_command(list(shaped)) - self.segment.observe(self.latest_feedback, stamp=self.state_receive_times[-1], now=now) + self.segment.observe(self.latest_feedback, stamp=self.state_history[-1].stamp_ns, now=now) self.reason = motion.phase+":"+str(motion.task_key) steady_complete = False if motion.phase == "steady": @@ -346,9 +612,9 @@ class CalibrationCoordinator: if r.get(field) == joint and r.get("view") == spec.view and r.get("sample_phase") == "steady"}) >= self.profile.acquisition.steady_minimum_samples for field, joint, spec in observation_streams(self.profile, task)) + steady_complete = steady_complete and self.segment.minimum_hold_complete(now) else: - self._steady_capture_after_ns = None - self._steady_rows.clear() + self._reset_steady_capture(motion, now) if motion.phase == "joint_zero": if self.segment.steady_ready(now): if self._steady_capture_after_ns is None: @@ -357,12 +623,49 @@ class CalibrationCoordinator: if session.phase == Phase.PAUSED: return else: - self._steady_capture_after_ns = None - self._steady_rows.clear() - if self.segment.arrived(now) and not steady_complete: - self._pause("joint_zero_unavailable:"+self._zero_reference_error) + self._reset_steady_capture(motion, now) + if self.branch_initialization.pending(): + # Optimizing the immutable preparation images is not a loss + # of current Tag observations. SDK feedback/safety keeps + # updating while this bounded operation holds the same pose. + self.reason = "joint_zero_geometry_solving:" + str(motion.task_key) + if (self._zero_geometry_started_at is not None + and now-self._zero_geometry_started_at > 120.0): + self._pause("joint_zero_motion_unresolved:image_motion_solver_timeout") return - if self.segment.arrived(now) or steady_complete: + # A long preparation solve can require the tracker to recover + # before any zero image is valid. Give that recovery the existing + # observation timeout, then collect the fixed number of stable + # frames within the dedicated zero window. Zero needs ten images; + # an ordinary mapping hold needs only three. Only the + # first accepted zero sample starts it; dropouts and feedback + # resets never extend it. Recovery plus capture remains bounded, + # with no relaxation of sample quality. + timeout = self.profile.acquisition.steady_timeout_seconds + capture_timeout = self.profile.acquisition.joint_zero_timeout_seconds + zero_started = self._zero_geometry_completed_at + if zero_started is not None and self._zero_capture_started_at is not None: + zero_started = min(self._zero_capture_started_at, zero_started+timeout) + timeout = capture_timeout + observation_timed_out = self.segment.arrived(now) and ( + zero_started is None or now-zero_started >= timeout) + if observation_timed_out and not steady_complete: + geometry_error = self.branch_initialization.error() + if geometry_error and self._complete_diagnostic_zero_attempt(motion, now, geometry_error): + steady_complete = True + elif geometry_error: + if self._recover_joint_zero(motion, now, geometry_error): + return + if self.diagnostic_capture is not None: + geometry_error += ":diagnostic_reference=" + self._zero_reference_error + self._pause("joint_zero_motion_unresolved:" + geometry_error) + return + else: + if self._recover_joint_zero(motion, now, ""): + return + self._pause("joint_zero_unavailable:"+self._zero_reference_error+self._joint_zero_branch_details(motion)) + return + if (self.segment.arrived(now) and motion.phase != "joint_zero") or steady_complete: if motion.phase == "reference_pose": waypoint = next(w for w in self.profile.motion.resume_verification_waypoints if w.key == motion.task_key) if self._reference_observation_started is None: @@ -371,19 +674,84 @@ class CalibrationCoordinator: required = [f"{waypoint.key}/{view.name}:{tag.role}" for view in self.profile.vision.views for tag in view.tags if tag.tag_id in waypoint.tag_ids_by_view.get(view.name, ())] - enough = all(len(self.capture.installations.get(key, ())) >= self.reference_lock.minimum_frames for key in required) + enough = all(self.capture.installation_count(key) >= self.reference_lock.minimum_frames for key in required) if not enough and now-self._reference_observation_started < 2.0: return # Missing installation evidence invalidates reuse, not the # entire fresh capture. Never forge a matching fingerprint. self._reference_capture_key = None + if self.segment.steady_ready(now) or ( + motion.phase not in {"steady", "joint_zero"} and self.segment.arrived(now)): + self._settled_pose = (tuple(motion.target), tuple(self.latest_feedback), now) self.execution.motion_complete() if phase == Phase.REFERENCE_POSES and session.phase != phase: self._save_reference() self.segment, self._motion = None, None + if session.phase == Phase.DIAGNOSTIC_COMPLETE: + self._finish_diagnostic_capture() + + def _reset_steady_capture(self, motion, now): + if self._steady_capture_after_ns is not None or self._steady_rows: + record = {"kind": "steady_capture_reset", "stamp_ns": self.ports.clock_ns(), + "task_name": motion.task_key, "motion_phase": motion.phase, + "discarded_joint_samples": sum(r.get("kind") in {"joint_sample", "joint_zero_sample"} + for r in self._steady_rows), + "feedback_window_seconds": self.profile.acquisition.steady_window_seconds, + "feedback_history": [{"age_seconds": now-stamp, "values": list(values)} + for stamp, values in list(self.segment.stamped_history)[-20:]]} + self.raw_records.append(record) + append_jsonl_many(self.raw_path, (record,), durable=False) + self._steady_capture_after_ns = None + self._steady_rows.clear() + + def _finish_diagnostic_capture(self): + """Save diagnostics under the state lock before exposing completion.""" + from .diagnostic_report import build_diagnostic_report + self._observation_epoch += 1 + try: + sync_jsonl(self.raw_path) + report = build_diagnostic_report(self.profile, self.diagnostic_capture, + raw_path=self.raw_path, records=self.raw_records, + references=self.execution.zero_references, + diagnostic_zero_attempts=self.execution.diagnostic_zero_attempts, + completed_units=self.execution.completed_units, + expected_units=self.execution.session.engine.scan_units(), + protected_inputs=self._current_fingerprint.get("protected_hashes", self.parameters.protected_inputs), + command=self.last_command, feedback=self.latest_feedback) + atomic_write_json(self.parameters.session_dir / "diagnostic_capture.json", report) + except (OSError, ValueError, TypeError, KeyError) as error: + self.execution.session.fail("diagnostic_capture_report_failed", str(error)) + self.reason = f"diagnostic_capture_report_failed:{error}" + return + self.reason = "diagnostic_capture_complete_not_calibration_acceptance" + + def _complete_diagnostic_zero_attempt(self, motion, now, geometry_error): + """Continue diagnostics only after a real stable raw reference visit.""" + if self.diagnostic_capture is None: + return False + if not self.segment.steady_ready(now): + self._zero_reference_error = "diagnostic_joint_zero_feedback_unstable" + return False + try: + attempt = build_diagnostic_zero_attempt(self.profile, motion, self._steady_rows, + reason=geometry_error, session_epoch=self._observation_epoch, + motion_version=self._segment_number) + except ValueError as error: + self._zero_reference_error = str(error) + return False + self.execution.record_diagnostic_zero_attempt(attempt) + row = attempt.as_record() + append_jsonl(self.raw_path, row) + self.raw_records.append(row) + return True def _lock_joint_zeros(self, motion): + if not self.branch_initialization.ready(motion): + self._zero_reference_error = (self.branch_initialization.error() + or "motion_geometry_confirmation_pending") + return False references = {} + branch_rows = [] try: for name in motion.zero_joints: rows = [r for r in self._steady_rows if r.get("kind") == "joint_zero_sample" and r.get("joint") == name] @@ -397,6 +765,9 @@ class CalibrationCoordinator: key=lambda r: r["image_stamp_ns"])[-minimum:]] references[name] = build_joint_zero_reference(self.profile, name, rows, session_epoch=self._observation_epoch, motion_version=self._segment_number) + branch_rows.extend(rows) + revisions = reference_branch_revisions(branch_rows, profile=self.profile, + require_motion_evidence=True) except ValueError as error: self._zero_reference_error = str(error) return False @@ -407,16 +778,117 @@ class CalibrationCoordinator: except ValueError as error: self._pause(str(error)) return False + moving = {(view.name, tag.role) for view in self.profile.vision.views + for tag in view.tags if not tag.fixed_reference} + if not SquareTagPoseTracker.freeze_references([ + (self.trackers[view], role, revision) for (view, role), revision in revisions.items() + ], require_motion_evidence=tuple((self.trackers[view], role) + for view, role in revisions if (view, role) in moving)): + self._zero_reference_error = "joint_zero_motion_branch_not_confirmed" + return False self.execution.zero_references.update(references) + provenance = self.joint_resume.take_motion_provenance_records() + append_jsonl_many(self.raw_path, provenance) + self.raw_records.extend(provenance) for reference in references.values(): row = reference.as_record() self.raw_records.append(row) append_jsonl(self.raw_path, row) return True + def _reuse_settled_preparation(self, motion, now): + if motion.phase != "prepare" or self._settled_pose is None: + return False + command, feedback, stamp = self._settled_pose + tolerance = .001 if self.command_unit == "rad" else 1. + if (tuple(motion.target) != command or tuple(self.last_command or ()) != command + or now-stamp > 1.0 or len(feedback) != len(self.latest_feedback) + or any(abs(old-new) > tolerance for old, new in zip(feedback, self.latest_feedback))): + return False + # The previous segment already completed its endpoint/feedback hold. + # Repeating an identical preparatory target needs no second dwell. + self.execution.motion_complete() + record = {"kind": "preparation_reused_settled_pose", "task_name": motion.task_key, + "cycle": motion.cycle, "direction": motion.direction, "stamp_ns": self.ports.clock_ns()} + append_jsonl_many(self.raw_path, (record,), durable=False) + self.raw_records.append(record) + return True + + def _prepare_command_sampling(self, task_key): + if task_key in self.execution.sampling_plans: + return + from ..core.fitting.command_sampling import build_sampling_plan + existing = [row for row in self.raw_records if row.get("kind") == "command_sampling_plan" + and row.get("task_name") == task_key] + if existing: + plan = existing[0] # PreparedResume already verified its source and content. + else: + task = next(task for task in self.profile.motion.tasks if task.key == task_key) + plan = build_sampling_plan(self.profile, task, self.raw_records) + append_jsonl(self.raw_path, plan) + self.raw_records.append(plan) + self.execution.sampling_plans[task_key] = plan + + def _recover_joint_zero(self, motion, now, geometry_error): + if self.diagnostic_capture is not None: + return False + action = self.zero_recovery.take(motion, geometry_error=geometry_error, + reports=self.branch_initialization.reports, referenced_joints=self.execution.zero_references) + if action is None: + return False + record = {"kind": "joint_zero_recovery", "task_name": motion.task_key, + "joints": list(motion.zero_joints), "action": action, "reason": geometry_error or self._zero_reference_error, + "attempt": 1, "stamp_ns": self.ports.clock_ns()} + append_jsonl(self.raw_path, record) + self.raw_records.append(record) + self._observation_epoch += 1 + self._steady_rows.clear() + self._zero_capture_started_at = None + self._steady_capture_after_ns = None + if action == "repeat_approach": + self.execution.repeat_zero_approach(self.last_command or motion.target) + self.branch_initialization.clear() + self._motion = self.segment = None + self._zero_geometry_started_at = self._zero_geometry_completed_at = None + else: + # Keep the frozen model and current physical holding pose. Only + # a fresh image window is opened; feedback and base guards run. + self._zero_geometry_completed_at = now + self.reason = "joint_zero_recovery:" + action + ":" + str(motion.task_key) + return True + + def _joint_zero_branch_details(self, motion): + task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + return self._task_branch_details(task, joints=motion.zero_joints) + + def _task_branch_details(self, task, *, joints=None): + from .scan_quality import observation_streams + details = [] + required_views = set() + for _, name, spec in observation_streams(self.profile, task): + if joints is not None and name not in joints: + continue + required_views.add(spec.view) + for role in (spec.parent_role, spec.child_role): + previous = self._branch_observation_reasons.get((spec.view, role)) + if previous is None: + continue + tag = next(tag for tag in self._view(spec.view).tags if tag.role == role) + details.append(f"{name}:{spec.view}:tag={tag.tag_id}:{previous[1]}") + for view in self.profile.vision.views: + if view.name not in required_views: + continue + for tag in view.tags: + previous = self._branch_observation_reasons.get((view.name, tag.role)) + if tag.fixed_reference and previous is not None: + details.append(f"fixed_reference:{view.name}:tag={tag.tag_id}:{previous[1]}") + return "" if not details else ":pose_branches="+",".join(sorted(set(details))) + def _begin_motion_capture(self, motion): - """Keep direction statistics across its separate steady checkpoints.""" + """Aggregate one direction across its moving and stationary segments.""" self._steady_rows = [] + self.branch_initialization.begin(motion, session_epoch=self._observation_epoch, + motion_version=self._segment_number) if motion.recording: key = (motion.task_key, motion.cycle, motion.direction, motion.attempt) if key != self._capture_unit_key: @@ -436,7 +908,8 @@ class CalibrationCoordinator: # These are the expected configuration identities, not live # measurements of serial numbers or physical camera geometry. "expected_cameras": {view: asdict(identity) - for view, identity in self.parameters.extrinsics.cameras.items()}, + for view, identity in self.parameters.extrinsics.cameras.items() + if view in self.required_observation_views}, **report.as_dict(), } atomic_write_json(self.parameters.session_dir / "camera_preflight.json", record) @@ -462,7 +935,7 @@ class CalibrationCoordinator: if self.ports.command_publisher_count() > 1: waiting.append("存在其他机械手指令发布者") cameras = {} - for view in self.profile.vision.view_names: + for view in self.required_observation_views: info_at = self.cameras.info_received_at.get(view) detection_at = self.cameras.detections_received_at.get(view) info_ready = (view in self.cameras.matrices and info_at is not None @@ -478,10 +951,30 @@ class CalibrationCoordinator: waiting.append(f"{view} 未收到新鲜的 Tag 检测消息（检查图像、整流和检测节点）") return DeviceReadiness(not waiting, tuple(waiting), cameras) - def _accept_feedback(self, names, positions, stamp: int) -> None: + def _accept_feedback(self, names, positions, stamp: int, *, channel_stamps_ns=()) -> StateSample | None: state = self.sdk_adapter.parse_feedback(names, positions) if state is None: return + if stamp <= 0 and not channel_stamps_ns: + stamp = self.ports.clock_ns() + if channel_stamps_ns: + from .adapters.base import ordered_feedback_values + ordered = ordered_feedback_values(self.sdk_adapter.command_layout, names, channel_stamps_ns) + if (ordered is None or any(type(value) is not int or value <= 0 for value in ordered) + or max(ordered) != stamp): + return + channel_stamps_ns = ordered + now_ns = self.ports.clock_ns() + if (max(ordered) > now_ns+self.parameters.maximum_state_image_skew_ns + or now_ns-min(ordered) > self.profile.acquisition.feedback_stale_seconds*1e9): + return + if self.state_history and stamp <= self.state_history[-1].stamp_ns: + return # Duplicate/out-of-order publication is not a new measurement. + if self.state_history and channel_stamps_ns: + previous = self.state_history[-1] + if previous.channel_stamps_ns and any(new < old or (new == old and value != old_value) + for new, old, value, old_value in zip(channel_stamps_ns, previous.channel_stamps_ns, state, previous.position_u8)): + return violation = next(( (index, value) for index, value in enumerate(state) @@ -500,12 +993,23 @@ class CalibrationCoordinator: f"upper={self.feedback_upper[index]:.9f}" ) return - if stamp <= 0: - stamp = self.ports.clock_ns() + received_at = self.ports.monotonic() if not self.state_history or stamp > self.state_history[-1].stamp_ns: - self.state_history.append(StateSample(stamp, state)) + self.state_history.append(StateSample(stamp, state, tuple(channel_stamps_ns))) + if self.segment is not None: + # Stability is a property of independent SDK observations, + # not of how often the control timer happened to run. + self.segment.observe(state, stamp=stamp, now=received_at) self.latest_feedback = state - self.state_receive_times.append(self.ports.monotonic()) + self.state_receive_times.append(received_at - max(0, self.ports.clock_ns()-min(channel_stamps_ns))/1e9 + if channel_stamps_ns else received_at) + if self.started and channel_stamps_ns and not self.finalization.started: + record = {"kind": "sdk_feedback_sample", "stamp_ns": stamp, + "received_stamp_ns": self.ports.clock_ns(), "source": "can_feedback_v1", + "state_values": list(state), "channel_stamps_ns": list(channel_stamps_ns)} + self.raw_records.append(record) + append_jsonl_many(self.raw_path, (record,), durable=False) + return self.state_history[-1] def _publish_command(self, values): if self.parameters.commands_enabled: @@ -551,10 +1055,11 @@ class CalibrationCoordinator: def _pause(self, reason: str) -> None: with self.step_data_lock: - if self.state in {"PASSED", "ABORTED"} or self._pause_effects_applied: + if self.state in {"PASSED", "DIAGNOSTIC_COMPLETE", "ABORTED", "FAILED"} or self._pause_effects_applied: return self.finalization.cancel() self._observation_epoch += 1 + self.branch_initialization.clear() self._pause_effects_applied = True if self.execution.session.phase != Phase.PAUSED: from .reporting.reasons_zh import reason_zh @@ -604,6 +1109,7 @@ class CalibrationCoordinator: tag.tag_id for tag in view.tags if tag.fixed_reference ) for view in self.profile.vision.views + if view.name in self.required_observation_views }, minimum_frames=self.profile.acquisition.fixed_reference_minimum_frames, maximum_corner_drift_px=( @@ -619,25 +1125,34 @@ class CalibrationCoordinator: self._current_fingerprint = {"profile_id": self.profile.key.profile_id, "acquisition_policy_version": self.execution.session.engine.checkpoint_token, "protected_hashes": {**self.parameters.protected_inputs, "intrinsics_sha256": hashlib.sha256( - json.dumps({v: m.tolist() for v, m in sorted(self.cameras.matrices.items())}, sort_keys=True).encode()).hexdigest()}, + json.dumps({v: self.cameras.matrices[v].tolist() + for v in sorted(self.required_observation_views)}, sort_keys=True).encode()).hexdigest()}, "fixed_corners_by_view": self.reference_lock.fingerprint(), "fixed_poses": fixed, "moving_tag_poses": moving} append_jsonl(self.raw_path, {"kind": "fixed_base_reference_locked", **self._current_fingerprint}) + def _import_resume_batch(self): + pending = self._resume_import + rows = pending.take_batch() + append_jsonl_many(self.raw_path, rows, durable=False) + self.raw_records.extend(rows) + if pending.complete: + # No scan is skipped until every imported row is durable. Between + # batches the ordinary feedback, abort and safety callbacks run. + sync_jsonl(self.raw_path) + self.execution.first_cycle_spans.update(self.joint_resume.spans_for_task(pending.task_key)) + self.execution.authorize_task_reuse(pending.units) + self._resumed_count += len(pending.units) + self._resume_import = None + def _verify_resume(self): decision = ResumeDecision(False, "基准变化，已放弃旧断点并重新采集") rows = [] try: - rows = load_capture(self.parameters.resume_raw_samples_path) - headers = [r for r in rows if r.get("kind") == "session_start"] - references = [r for r in rows if r.get("kind") == "fixed_base_reference_locked"] - if len(headers) != 1 or len(references) != 1: - raise ValueError("missing unique checkpoint header/reference") - header = headers[0] - if header.get("serial_number") != self.parameters.serial_number or not self.execution.session.engine.resume_compatible(header): - raise ValueError("checkpoint identity/policy changed") - complete = {(r["task_name"], r["cycle"], r["direction"]) for r in rows - if r.get("kind") == "scan_unit_complete" and r.get("passed") is True} + checkpoint = self._resume_checkpoint + if checkpoint is None: + raise ValueError(self._resume_checkpoint_error or "resume_checkpoint_not_prepared") + rows, header, complete = checkpoint.rows, checkpoint.header, checkpoint.completed_units used_tasks = {key[0] for key in complete} from .scan_quality import observation_streams required = {f"{spec.view}:{role}" for task in self.profile.motion.tasks if task.key in used_tasks @@ -649,7 +1164,7 @@ class CalibrationCoordinator: verifier = ResumeVerifier(required_fixed_views=self.profile.vision.view_names, required_fixed_poses=self.profile.vision.view_names, required_moving_poses=required, required_hashes=self._current_fingerprint["protected_hashes"]) - previous = {**references[0], "acquisition_policy_version": header["acquisition_policy_version"]} + previous = {**checkpoint.fixed_reference, "acquisition_policy_version": header["acquisition_policy_version"]} from .resume import select_installation_evidence old, new = select_installation_evidence(fingerprint_from_mapping(previous), fingerprint_from_mapping(self._current_fingerprint), required) @@ -657,19 +1172,18 @@ class CalibrationCoordinator: if decision.reuse: from dataclasses import replace if self.profile.artifacts.output_schema_version >= 3: - self.joint_resume.stage(self.execution.session.engine, rows, complete) + self.joint_resume = checkpoint.joints decision = replace(decision, reason="整场参考通过，等待逐关节验证原零位后复用") else: decision = replace(decision, completed_units=tuple(sorted(complete))) except (OSError, ValueError, KeyError, TypeError) as error: - decision = ResumeDecision(False, decision.reason, (str(error),)) + decision = ResumeDecision(False, "旧断点校验未通过，已重新采集", (str(error),)) decision = self.execution.restore(decision, rows) if decision.reuse: used = set(decision.completed_units) imported = [r for r in rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in used] - for row in imported: - self.raw_records.append(row) - append_jsonl(self.raw_path, row) + append_jsonl_many(self.raw_path, imported) + self.raw_records.extend(imported) self._resumed_count = len(used) self._resume_message = decision.reason append_jsonl(self.raw_path, {"kind": "resume_verification", "reuse": decision.reuse, @@ -686,13 +1200,24 @@ class CalibrationCoordinator: return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed def _finalize(self) -> None: + if self.diagnostic_capture is not None: + self._pause("diagnostic_capture_not_for_publication_or_resume") + return if self.finalization.started or self.execution.session.phase != Phase.FIT: return + sync_jsonl(self.raw_path) + # Closing acquisition freezes the already collected camera/reference + # evidence. Later images cannot invalidate or mutate the fit input; + # feedback, device safety and explicit abort remain live in _advance. + self._observation_epoch += 1 + self.branch_initialization.clear() self.reason = "fitting_validating_and_writing_artifacts" self.finalization.start(FinalizationInputs( self.parameters.session_dir, self.parameters.serial_number, self.parameters.source_urdf, self._current_fingerprint.get("protected_hashes", self.parameters.protected_inputs), - tuple(self.raw_records), + tuple(self.raw_records) + tuple(self.cameras.models.values()), require_motion_evidence=True, + journal_path=self.raw_path, journal_size=self.raw_path.stat().st_size, + camera_extrinsics_file=self.parameters.camera_extrinsics_file, )) def _poll_finalization(self) -> None: @@ -702,14 +1227,10 @@ class CalibrationCoordinator: return correction = result[2] except FinalizationProtocolError as error: - self._pause(f"finalization_protocol_error:{error}") + self._fail_finalization("finalization_protocol_error", error) return except Exception as error: - atomic_write_json(self.parameters.session_dir / "failure_diagnostic.json", { - "profile_id": self.profile.key.profile_id, "reason": str(error), - "traceback": traceback.format_exc(), "source_urdf_unchanged": True, - }) - self._pause(f"fit_or_publication_failed:{error}") + self._fail_finalization("fit_or_publication_failed", error) return self.execution.session.published() self.reason = "partial_calibration_passed" if self.profile.scope.default_scope != "full" else "full_calibration_passed" @@ -717,6 +1238,33 @@ class CalibrationCoordinator: serial_number=self.parameters.serial_number, side=self.profile.key.side, model=self.model_name.lower())) self.final_urdf = str(correction.path) + def _fail_finalization(self, code: str, error: Exception) -> None: + """End a completed acquisition without suggesting another motion run. + + Numeric, final-file and worker-protocol failures share this boundary. + Feedback/safety failures during motion still use the ordinary pause. + Cancellation and epoch invalidation precede diagnostic I/O so even an + unwritable report cannot leave a worker authorized to publish. + """ + stage = self.execution.session.phase.value + self.finalization.cancel() + self._observation_epoch += 1 + self.branch_initialization.clear() + self._pause_effects_applied = True + self.reason = f"{code}:{error}" + details = {"stage": stage, "raw_reason": self.reason, + "raw_samples": str(self.raw_path), + "completed_unit_count": len(self.execution.completed_units), + "expected_unit_count": len(self.execution.session.engine.scan_units())} + self.execution.session.fail(code, "采集已结束，拟合或文件验收失败", details, + suggestion="保留原始采集和配置，先离线复算并检查失败诊断；仅在证据不足时重新采集。") + if self.last_command is not None: + self._publish_command(list(self.last_command)) + diagnostic = {"profile_id": self.profile.key.profile_id, "reason": str(error), + "state": "FAILED", **details, "traceback": traceback.format_exc()} + atomic_write_json(self.parameters.session_dir / "failure_diagnostic.json", diagnostic) + append_jsonl(self.raw_path, {"kind": "finalization_failed", **diagnostic}) + def _finalizer_event(self, event: str) -> None: session = self.execution.session if event == "fit_complete": @@ -731,5 +1279,8 @@ class CalibrationCoordinator: raise ValueError(f"unknown finalizer event:{event}") def _authorize_commit(self) -> None: + if self.diagnostic_capture is not None: + raise RuntimeError("diagnostic_capture_not_for_publication_or_resume") if self.execution.session.phase != Phase.PUBLISH: raise RuntimeError("finalization did not complete all acceptance stages") + sync_jsonl(self.raw_path) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_dispatch.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_dispatch.py new file mode 100644 index 0000000..3152d3a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_dispatch.py @@ -0,0 +1,98 @@ +"""Bounded image work outside ROS callbacks, with measurement-time lookahead.""" + +from concurrent.futures import ThreadPoolExecutor +from threading import Condition +import math +import time + + +class DetectionDispatcher: + """One active and one pending image per view; SDK callbacks remain short. + + A detected image can arrive before the matching CAN observation is + delivered. Wait at most the existing synchronization tolerance for that + observation, without blocking ROS feedback or motor commands. The capture + layer still enforces its original timestamp and observation-quality gates. + """ + + def __init__(self, views, deliver, *, maximum_wait_seconds, failed): + self._deliver, self._failed = deliver, failed + self._maximum_wait = float(maximum_wait_seconds) + if not math.isfinite(self._maximum_wait) or self._maximum_wait <= 0 or not views or len(set(views)) != len(views): + raise ValueError("invalid image dispatch views or synchronization wait") + self._condition = Condition() + self._pending = dict.fromkeys(views) + self._generation, self._closed = 0, False + self._feedback_stamp_ns = 0 + self._error = None + self._executor = ThreadPoolExecutor(max_workers=len(views), thread_name_prefix="calibration_image") + self._workers = [self._executor.submit(self._guarded_run, view) for view in views] + + def _guarded_run(self, view): + try: + self._run(view) + except Exception as error: + with self._condition: + self._error = error + self._failed(error) + + def raise_if_failed(self): + with self._condition: + if self._error is not None: + raise RuntimeError("image dispatch failed") from self._error + + def submit(self, observation): + with self._condition: + if self._closed: + return + if observation.view not in self._pending: + raise ValueError("undeclared image dispatch view") + self._pending[observation.view] = observation + self._condition.notify_all() + + def feedback_available(self, stamp_ns): + """Advance to the oldest channel of an accepted measurement snapshot. + + The queue never calls back into the coordinator while holding its + condition. Control messages can therefore wake image workers without + waiting for another worker to acquire the session lock. + """ + with self._condition: + self._feedback_stamp_ns = max(self._feedback_stamp_ns, stamp_ns) + self._condition.notify_all() + + def clear(self): + with self._condition: + self._generation += 1 + self._feedback_stamp_ns = 0 + self._pending = dict.fromkeys(self._pending) + self._condition.notify_all() + + def _run(self, view): + while True: + with self._condition: + self._condition.wait_for(lambda: self._closed or self._pending[view] is not None) + if self._closed: + return + observation, generation = self._pending[view], self._generation + self._pending[view] = None + deadline = time.monotonic()+self._maximum_wait + while not self._closed and generation == self._generation: + if self._feedback_stamp_ns >= observation.stamp_ns: + break + remaining = deadline-time.monotonic() + if remaining <= 0: + break + self._condition.wait(timeout=remaining) + if self._closed: + return + if generation != self._generation: + continue + self._deliver(observation) + + def close(self): + with self._condition: + self._closed = True + self._pending = dict.fromkeys(self._pending) + self._condition.notify_all() + self._executor.shutdown(wait=True, cancel_futures=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py new file mode 100644 index 0000000..87cbbd3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py @@ -0,0 +1,298 @@ +"""Analyze one stopped capture, without moving hardware or authorizing poses. + +Each acquisition stage and session keeps its own image identities and mounting +context. This report is separate from calibration artifacts and the original +capture report; none of its fitted geometry can enter the live reference path. +""" + +from __future__ import annotations + +import argparse +from collections import defaultdict +from dataclasses import asdict +import hashlib +import json +import math +from pathlib import Path + +from ..core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates +from ..core.geometry.tag_pose.types import SquareTagPose +from ..storage import atomic_write_json +from .motion_provenance import source_frame_sha256 +from .scan_quality import observation_streams + + +def _file_sha256(path): + digest = hashlib.sha256() + with path.open("rb") as source: + for block in iter(lambda: source.read(1024 * 1024), b""): + digest.update(block) + return digest.hexdigest() + + +def _check_output_targets(raw_path, output): + raw_path = raw_path.resolve() + for name in ("diagnostic_analysis.json", "diagnostic_analysis.md"): + target = output / name + if target.resolve() == raw_path or (target.exists() and target.samefile(raw_path)): + raise ValueError("diagnostic_analysis_output_overwrites_source") + + +def _pose(payload): + return SquareTagPose(tuple(payload["quaternion_xyzw"]), + tuple(payload["translation_xyz_m"]), float(payload["reprojection_error_px"])) + + +def image_frames(records, relations): + """Convert detached, same-image evidence; never substitute a later pose.""" + from ..core.geometry.tag_pose.motion_image_diagnostics import ImageMotionFrame, ImageRoleObservation + + roots = {relation.parent_role for relation in relations} - {relation.child_role for relation in relations} + roles = tuple(dict.fromkeys(role for relation in relations + for role in (relation.parent_role, relation.child_role))) + frames = [] + for row in records: + stamp = row["image_stamp_ns"] + candidates, observations = [], [] + for role in roles: + item = row.get("tags", {}).get(role) + if item is None or item.get("candidate_diagnostics", {}).get("observation_stamp_ns") != stamp: + break + diagnostics = item["candidate_diagnostics"] + # This offline comparison may inspect rejected moving candidates, + # including old captures. Preserve the verified root, but never + # reinterpret this counterfactual fit as a new live authorization. + frozen = role in roots and diagnostics.get("branch_frozen") is True + payloads = ([item["selected_pose"]] if frozen and item.get("selected_pose") is not None + else [] if frozen else item.get("reprojection_valid_candidates", ())) + if not payloads or item.get("corners_xy") is None: + break + candidates.append(RoleCandidates(role, tuple(_pose(p) for p in payloads), frozen)) + observations.append(ImageRoleObservation(role, float(item["tag_size_m"]), + tuple(tuple(point) for point in item["corners_xy"]))) + else: + frames.append(ImageMotionFrame(MotionEvidenceFrame(stamp, tuple(candidates)), + tuple(tuple(values) for values in row["camera_matrix"]), tuple(observations))) + return tuple(frames) + + +def _stages(profile, rows): + observations = {} + sweeps = defaultdict(dict) + headers = [] + initializations = [] + for row in rows: + kind = row.get("kind") + if kind == "session_start": + headers.append(row) + if kind == "motion_branch_initialization": + initializations.append(row) + if kind not in {"motion_branch_observation", "diagnostic_observation_frame"}: + continue + identity = (row["view"], row["image_stamp_ns"]) + if identity in observations and observations[identity] != row: + raise ValueError("diagnostic_analysis_image_identity_changed") + observations[identity] = row + if kind == "diagnostic_observation_frame" and row.get("sample_phase") == "sweep": + if row.get("cycle") == 3: + raise ValueError("diagnostic_analysis_refuses_holdout_training") + key = (row["task_name"], row["view"], row["cycle"], row["direction"], row.get("attempt", 1)) + sweeps[key][row["image_stamp_ns"]] = row + if len(headers) != 1 or headers[0].get("profile_id") != profile.key.profile_id: + raise ValueError("diagnostic_analysis_profile_or_session_mismatch") + tasks = {task.key: task for task in profile.motion.tasks} + stages = [] + for number, initial in enumerate(initializations): + task = tasks[initial["task_name"]] + relations = tuple(MotionRelation(name, spec.parent_role, spec.child_role) + for _, name, spec in observation_streams(profile, task) + if name in initial["zero_joints"] and spec.view == initial["view"]) + selected = [] + for stamp in initial["source_image_stamps"]: + row = observations.get((initial["view"], stamp)) + if row is None or any(row.get(key) != initial.get(key) + for key in ("task_name", "zero_joints", "session_epoch", "motion_version")): + raise ValueError("diagnostic_analysis_initialization_source_mismatch") + expected = initial.get("source_frame_hashes", {}).get(str(stamp)) + if expected is None or source_frame_sha256(row) != expected: + raise ValueError("diagnostic_analysis_initialization_hash_mismatch") + selected.append(row) + stages.append(({"key": f"initialization:{number}", "task_name": task.key, + "view": initial["view"], "sample_phase": "zero_approach", + "recorded_branch_status": initial["status"], + "recorded_branch_reason": initial["reason"], + "recorded_hypotheses": initial["hypotheses"]}, relations, tuple(selected))) + for (task_key, view, cycle, direction, attempt), images in sorted(sweeps.items()): + task = tasks[task_key] + relations = tuple(MotionRelation(name, spec.parent_role, spec.child_role) + for _, name, spec in observation_streams(profile, task) if spec.view == view) + if not relations: + continue # Retained camera context is not a task measurement. + stages.append(({"key": f"sweep:{task_key}:{view}:{cycle}:{direction}:{attempt}", + "task_name": task_key, "view": view, "sample_phase": "sweep", + "cycle": cycle, "direction": direction, "attempt": attempt}, relations, + tuple(row for _, row in sorted(images.items())))) + return headers[0], stages + + +def _image_tables(report): + lines = [] + labels = {"interleaved": "交错留出", "early_to_late": "前段训练→后段验证", + "late_to_early": "后段训练→前段验证"} + for hypothesis in report.get("hypotheses", ()): + label = ", ".join(f"{role}={index}" for role, index in hypothesis["branches"]) + lines += [f"### 候选 {label}", ""] + if hypothesis.get("reason"): + lines += [f"分析限制：`{hypothesis['reason']}`。", ""] + pnp = hypothesis.get("pnp_geometry", ()) + if pnp: + lines += ["| 关节 | 原 PnP 转心 RMS | 原 PnP 位置 P95 | 深度误差占比 |", + "| --- | ---: | ---: | ---: |"] + lines += [f"| {item['joint']} | {item['translation_rms_m']*1000:.3f} mm | " + f"{item['translation_p95_m']*1000:.3f} mm | {item['camera_depth_squared_error_fraction']*100:.1f}% |" + for item in pnp] + lines.append("") + intervals = hypothesis.get("intervals", ()) + if intervals: + lines += ["| 独立图像检查 | Tag | 留出图像数 | RMS | 逐帧 RMS 的 P95 | 优化收敛 |", + "| --- | --- | ---: | ---: | ---: | --- |"] + for interval in intervals: + state = "是（不代表精度通过）" if interval["converged"] else "否" + for item in interval.get("validation_quality", ()): + lines.append(f"| {labels.get(interval['name'], interval['name'])} | {item['role']} | " + f"{item['frames']} | {item['rms_px']:.3f} px | {item['frame_rms_p95_px']:.3f} px | {state} |") + lines.append("") + if hypothesis.get("repeatability"): + lines += ["| 关节 | 前后段转轴方向差 |", "| --- | ---: |"] + lines += [f"| {item['joint']} | {math.degrees(item['early_late_axis_difference_rad']):.3f}° |" + for item in hypothesis["repeatability"]] + lines += ["", "轴方向差可与现行分支检查的 3° 要求对照；该图像诊断不会代替原验收。", ""] + return lines + + +def _summary_markdown(report): + lines = ["# 原始观测的图像几何诊断", "", + "本报告只解释原始角点，不授权姿态分支、关节零位、JSON 或 URDF。", + "各任务和方向分别拟合；未跨会话拼接，也未作为第四轮精度验收。", "", + f"来源：`{report['raw_samples_path']}`", f"SHA-256：`{report['raw_samples_sha256']}`", ""] + if report.get("missing_evidence"): + lines += ["尚缺以下记录，当前报告不能补出这些真实观测：", ""] + lines += [f"- `{item}`" for item in report["missing_evidence"]] + lines.append("") + if report.get("status") == "failed": + lines += [f"分析未完成：`{report['reason']}`", "原始采集数据仍然保留。"] + for stage in report.get("stages", ()): + lines += [f"## {stage['task_name']} / {stage['view']} / {stage.get('direction', '归零准备')}", "", + f"原始图像 {stage['source_image_count']} 张，可用完整图像 {stage['usable_image_count']} 张。"] + if "recorded_branch_status" in stage: + lines.append(f"原分支判定：`{stage['recorded_branch_status']}`；原因：`{stage['recorded_branch_reason']}`。") + image = stage["image_diagnostics"] + lines += [f"图像诊断：`{image['status']}`；原因：`{image.get('reason', '')}`。", "", + "候选编号只在本阶段有效。下列所有候选均保留，没有把最低分候选写成正确零位。", ""] + lines.extend(_image_tables(image)) + lines += ["## 如何使用", "", + "- 采集完成只代表原始观测已保存；未确认关节仍没有有效零位和角度样本。", + "- 图像误差较小不能换算成毫米精度通过，也不能单凭最低分选择一个分支。", + "- 分段模型不一致或多个候选仍可解释图像时，保留未确认结论。", + "- 正式标定仍须满足原有几何、JSON 映射和独立第四轮验收。", ""] + return "\n".join(lines) + + +def _missing_evidence(profile, header, stages): + missing = [] if stages else ["no_recorded_motion_images"] + diagnostic = header.get("diagnostic_capture") + if not diagnostic: + return missing + observed = {(stage["task_name"], stage["view"], stage.get("cycle"), stage.get("direction")) + for stage in stages if stage["sample_phase"] == "sweep"} + for task in profile.motion.tasks: + if task.key not in diagnostic.get("task_keys", ()): + continue + views = {spec.view for _, _, spec in observation_streams(profile, task)} + for view in sorted(views): + for cycle in diagnostic.get("cycles", ()): + for direction in ("decreasing", "increasing"): + if (task.key, view, cycle, direction) not in observed: + missing.append(f"sweep:{task.key}:{view}:{cycle}:{direction}") + if not any(stage["sample_phase"] == "zero_approach" for stage in stages): + missing.append("no_pre_zero_motion_record; old_sweeps_cannot_replace_initialization") + return missing + + +def analyze_capture(profile, raw_path: Path, *, output_dir: Path | None = None, progress=None): + """Generate bounded per-stage image diagnostics after the ROS stack stops.""" + from ..core.geometry.tag_pose.motion_image_diagnostics import analyze_motion_images + + raw_path = raw_path.resolve() + output = raw_path.parent if output_dir is None else output_dir.resolve() + _check_output_targets(raw_path, output) + before = _file_sha256(raw_path) + with raw_path.open(encoding="utf-8") as source: + rows = [json.loads(line) for line in source if line.strip()] + header, stages = _stages(profile, rows) + analyzed = [] + for index, (identity, relations, records) in enumerate(stages): + if progress is not None: + progress(f"分析观测 {index+1}/{len(stages)}：{identity['task_name']} / {identity['view']} / " + + identity.get("direction", "归零准备")) + frames = image_frames(records, relations) + result = analyze_motion_images(frames, relations) + analyzed.append({**identity, "source_image_count": len(records), + "usable_image_count": len(frames), "image_diagnostics": asdict(result)}) + if _file_sha256(raw_path) != before: + raise ValueError("diagnostic_analysis_source_changed_during_read") + report = {"format": "calibration_image_diagnostic_v1", "schema_version": 1, + "scope": "diagnostic_not_branch_authorization", "status": "complete", + "accuracy_validated": False, "publication_allowed": False, + "moving_candidates_counterfactual": True, + "raw_samples_path": str(raw_path), "raw_samples_sha256": before, + "profile_id": profile.key.profile_id, + "source_protected_inputs": {key: value for key, value in header.items() if key.endswith("_sha256")}, + "stages": analyzed, "missing_evidence": _missing_evidence(profile, header, analyzed), + "limitations": ["Independent sessions are never combined.", + "Per-stage image fits do not authorize a branch or certify 3D accuracy.", + "Training images are not independent validation; split metrics are reported separately."]} + output.mkdir(parents=True, exist_ok=True) + atomic_write_json(output / "diagnostic_analysis.json", report) + (output / "diagnostic_analysis.md").write_text(_summary_markdown(report), encoding="utf-8") + return report + + +def finish_diagnostic_analysis(profile, session: Path): + """Analysis failure must not relabel a completed capture as an SDK failure.""" + try: + report = analyze_capture(profile, session / "raw_samples.jsonl", progress=lambda message: print(message, flush=True)) + except Exception as error: + # This is the boundary between a completed acquisition and optional + # offline computation, including failures raised by numerical libraries. + # KeyboardInterrupt/SystemExit still propagate to the runner. + report = {"status": "failed", "scope": "diagnostic_not_branch_authorization", + "reason": str(error), "accuracy_validated": False, "publication_allowed": False, + "raw_samples_path": str(session / "raw_samples.jsonl"), "raw_samples_sha256": ""} + try: + _check_output_targets(session / "raw_samples.jsonl", session) + atomic_write_json(session / "diagnostic_analysis.json", report) + (session / "diagnostic_analysis.md").write_text(_summary_markdown(report), encoding="utf-8") + except (OSError, ValueError) as write_error: + print(f"离线分析结果也无法保存：{write_error}。原始采集状态保持不变。", flush=True) + print(f"原始采集已完成，但离线图像分析未完成：{error}。数据已保留，可离线复算。", flush=True) + print(f"图像几何诊断：{session / 'diagnostic_analysis.md'}", flush=True) + return report + + +def main(args=None): + from ..product import load_product_config + + parser = argparse.ArgumentParser(description="离线分析已保存的运动 Tag 角点，不启动实机、不发布标定产物") + parser.add_argument("--config", required=True) + parser.add_argument("--raw", required=True) + parser.add_argument("--output", default=None) + selected = parser.parse_args(args) + config = load_product_config(selected.config, check_can=False) + analyze_capture(config.calibration_contract.typed_profile, Path(selected.raw), + output_dir=None if selected.output is None else Path(selected.output), + progress=lambda message: print(message, flush=True)) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_capture.py new file mode 100644 index 0000000..1800c48 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_capture.py @@ -0,0 +1,112 @@ +"""Explicit, non-publishing capture presets over the shared motion schedule.""" + +from dataclasses import asdict, dataclass +import math +from numbers import Real +from typing import Mapping, Sequence + +from ..core.domain.profile import CalibrationProfile +from ..profiles.diagnostics import DIAGNOSTIC_CAPTURE_PRESETS +from ..acquisition import StateSample + + +def _finite_number(value) -> bool: + return isinstance(value, Real) and not isinstance(value, bool) and math.isfinite(value) + + +@dataclass(frozen=True) +class DiagnosticCapturePlan: + mode: str + task_keys: tuple[str, ...] + cycles: tuple[int, ...] = (0,) + hold_command_values: tuple[float, ...] = () + hold_seconds: float = 0.0 + + def __post_init__(self) -> None: + # The declared diagnostic allows at most ten seconds per arrival; + # extending that motion contract requires an explicit preset review. + if (not _finite_number(self.hold_seconds) + or (not self.hold_command_values and self.hold_seconds != 0.0) + or (self.hold_command_values and not 0.0 < self.hold_seconds <= 10.0)): + raise ValueError("diagnostic_hold_seconds_invalid") + if (not isinstance(self.hold_command_values, tuple) + or any(not _finite_number(value) for value in self.hold_command_values) + or any(left >= right for left, right in zip( + self.hold_command_values, self.hold_command_values[1:]))): + raise ValueError("diagnostic_hold_values_invalid") + + def as_dict(self) -> dict: + result = asdict(self) + if not self.hold_command_values: + result.pop("hold_command_values") + result.pop("hold_seconds") + return result + + +def diagnostic_capture_label(mode: str) -> str: + preset = DIAGNOSTIC_CAPTURE_PRESETS.get(mode) + return preset.label_zh if preset else "诊断采集" + + +def diagnostic_hold_timing(history: Sequence[StateSample], *, target: tuple[float, ...], + image_stamp_ns: int, session_epoch: int, motion_version: int, motion_started_ns: int, + feedback_stable_since_ns: int | None) -> dict: + """Describe the actual held command without assigning unmeasured exposure time. + + Only command samples at or before this image are considered. A rounded u8 + target may be issued before the trajectory finishes, so its first issue + is retained separately from the motion's start and feedback stability. + """ + first_target = next((sample.stamp_ns for sample in history + if motion_started_ns <= sample.stamp_ns <= image_stamp_ns + and tuple(sample.position_u8) == target), None) + stable_since = feedback_stable_since_ns + if stable_since is not None and not motion_started_ns <= stable_since <= image_stamp_ns: + stable_since = None + return { + "hold_id": f"{session_epoch}:{motion_version}", + "target_first_command_stamp_ns": first_target, + "feedback_stable_since_ns": stable_since, + } + + +def resolve_diagnostic_capture(profile: CalibrationProfile, mode: str) -> DiagnosticCapturePlan | None: + """Select an audited task subset without changing a protected Profile. + + Presets select tasks and optional directional holds. Safety, references + and vision remain the common implementations; no alternative fitter is + created, and these observations cannot certify calibration artifacts. + """ + if not mode: + return None + preset = DIAGNOSTIC_CAPTURE_PRESETS.get(mode) + if preset is None or profile.key != preset.profile_key: + raise ValueError(f"unsupported_diagnostic_capture:{mode}:{profile.key.profile_id}") + expected = dict(preset.tasks) + tasks = tuple(task for task in profile.motion.tasks if task.key in expected) + if len(tasks) != len(expected) or profile.artifacts.output_schema_version < 3: + raise ValueError("diagnostic_thumb_tasks_or_joint_zeros_missing") + for task in tasks: + if (set(task.joints) != set(expected[task.key]) or task.validation_only + or task.command_index != profile.command.command_index_by_joint.get(expected[task.key][0]) + or any(name not in profile.motion.joint_zero_references + or profile.motion.joint_zero_references[name].task_key != task.key for name in task.joints)): + raise ValueError(f"diagnostic_thumb_task_binding_changed:{task.key}") + plan = DiagnosticCapturePlan(mode, tuple(task.key for task in tasks), + hold_command_values=preset.hold_command_values, hold_seconds=preset.hold_seconds) + for task in tasks: + lower = max(min(task.start_value, task.end_value), + profile.command.minimum_values[task.command_index]) + upper = min(max(task.start_value, task.end_value), + profile.command.maximum_values[task.command_index]) + if any(not lower < value < upper for value in plan.hold_command_values): + raise ValueError(f"diagnostic_hold_outside_task_domain:{task.key}") + return plan + + +def reject_diagnostic_capture(records: Sequence[Mapping]) -> None: + """Diagnostic samples must never be promoted to formal fit or resume data.""" + if any((row.get("kind") == "session_start" and row.get("diagnostic_capture")) + or row.get("kind") in {"diagnostic_observation_frame", "diagnostic_joint_zero_attempt"} + or row.get("quality_scope") == "diagnostic_observation_coverage" for row in records): + raise ValueError("diagnostic_capture_not_for_publication_or_resume") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_quality.py new file mode 100644 index 0000000..0e39766 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_quality.py @@ -0,0 +1,84 @@ +"""Separate diagnostic image coverage from accepted-pose calibration quality.""" + +from dataclasses import dataclass +from collections.abc import Mapping, Sequence +import math + +import numpy as np + +from ..core.domain.profile import CalibrationProfile +from .engine import ScanUnit, SweepQuality +from .scan_quality import evaluate_capture_unit, observation_streams +from .observation_timing import summarize_observation_timing + + +@dataclass(frozen=True) +class DiagnosticUnitQuality: + observations: SweepQuality + accepted_poses: SweepQuality + observation_timing: Mapping[str, Mapping] + + def accepted_pose_report(self) -> dict: + return {"passed": self.accepted_poses.passed, + "failures": list(self.accepted_poses.failures), + "metrics": dict(self.accepted_poses.metrics), "accuracy_validated": False} + + +def _complete_vector(values, count: int) -> bool: + return (isinstance(values, (list, tuple)) and len(values) == count + and all(isinstance(value, (int, float)) and math.isfinite(value) for value in values)) + + +def _visible_roles(row: Mapping, tags: Mapping) -> set[str]: + visible = set() + for role, item in row.get("tags", {}).items(): + tag = tags.get(role) + if (tag is None or item.get("tag_role") != role or item.get("tag_id") != tag.tag_id + or item.get("candidate_diagnostics", {}).get("observation_stamp_ns") != row.get("image_stamp_ns")): + continue + points = np.asarray(item.get("corners_xy"), dtype=float) + if points.shape == (4, 2) and np.all(np.isfinite(points)): + visible.add(role) + return visible + + +def evaluate_diagnostic_unit(profile: CalibrationProfile, unit: ScanUnit, attempt: int, + records: Sequence[Mapping], +) -> DiagnosticUnitQuality: + """Same motion/coverage limits; ambiguous poses are evidence for diagnosis. + + Temporary records carry feedback coverage only, never measured angles. + They remain local to this check and cannot be serialized as joint samples. + Missing current moving-Tag corners still fail the ordinary one-retry gate. + """ + accepted = evaluate_capture_unit(profile, unit, attempt, records, + first_cycle_spans={}, include_steady=False) + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + tags_by_view = {view.name: {tag.role: tag for tag in view.tags} for view in profile.vision.views} + required = {} + for field, name, spec in observation_streams(profile, task): + moving_roles = {tag.role for tag in tags_by_view[spec.view].values() if not tag.fixed_reference} + required[(field, name, spec.view)] = moving_roles & {spec.parent_role, spec.child_role} + coverage = [row for row in records if row.get("kind") != "diagnostic_observation_frame"] + for row in records: + if (row.get("kind") != "diagnostic_observation_frame" + or row.get("sample_phase") != "sweep" + or (row.get("task_name"), row.get("cycle"), row.get("direction"), row.get("attempt")) + != (unit.task_key, unit.cycle, unit.direction, attempt)): + continue + feedback = row.get("feedback_vector") + if (row.get("command_unit") != profile.command.unit + or not _complete_vector(feedback, profile.command.command_count) + or not _complete_vector(row.get("command_vector"), profile.command.command_count)): + continue + visible = _visible_roles(row, tags_by_view.get(row.get("view"), {})) + for (field, name, view), roles in required.items(): + if row.get("view") == view and roles and roles <= visible: + coverage.append({field: name, "view": view, "task_name": unit.task_key, + "cycle": unit.cycle, "direction": unit.direction, "attempt": attempt, + "image_stamp_ns": row["image_stamp_ns"], "sample_phase": "sweep", + f"feedback_{profile.command.unit}": feedback[task.command_index]}) + observed = evaluate_capture_unit(profile, unit, attempt, coverage, + first_cycle_spans={}, include_steady=False) + return DiagnosticUnitQuality(observed, accepted, + summarize_observation_timing(profile, unit, attempt, records)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py new file mode 100644 index 0000000..eb4faa7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py @@ -0,0 +1,100 @@ +"""Unresolved reference observations cannot become calibrated joint zeros. + +The diagnostic runner may finish its declared motion with raw corner evidence. +This record proves only that the reference pose was visited and observed; it +does not supply an angle, a mounting transform, or permission to publish. +""" + +from dataclasses import asdict, dataclass +import math + +import numpy as np + +from .scan_quality import observation_streams + + +@dataclass(frozen=True) +class DiagnosticZeroAttempt: + joint_names: tuple[str, ...] + task_name: str + reason: str + session_epoch: int + motion_version: int + command_vector: tuple[float, ...] + feedback_vector: tuple[float, ...] + source_images: tuple[tuple[str, int], ...] + joint_views: tuple[tuple[str, str], ...] + + def as_record(self): + return {"kind": "diagnostic_joint_zero_attempt", "schema_version": 1, + "status": "unresolved", "no_zero_locked": True, + "is_accepted_joint_sample": False, **asdict(self)} + + +def _complete_vector(values, count): + return (isinstance(values, (list, tuple)) and len(values) == count + and all(isinstance(v, (float, int)) and not isinstance(v, bool) + and math.isfinite(v) for v in values)) + + +def build_diagnostic_zero_attempt(profile, motion, records, *, reason, + session_epoch, motion_version): + """Check current raw images after the caller's SDK steady-window gate. + + Each moving parent/child must have real current-image corners. A stationary + base may use the already frozen session reference, as during normal scans. + Pose selection and geometric stability are intentionally not certified. + """ + if motion.phase != "joint_zero" or not motion.zero_joints or not reason: + raise ValueError("diagnostic_joint_zero_attempt_invalid") + task = next(task for task in profile.motion.tasks if task.key == motion.task_key) + declared = profile.motion.joint_zero_references + if (not set(motion.zero_joints) <= set(task.joints) + or any(name not in declared or declared[name].task_key != motion.task_key + or declared[name].command != motion.target for name in motion.zero_joints)): + raise ValueError("diagnostic_joint_zero_declared_pose_mismatch") + streams = [(name, spec) for _, name, spec in observation_streams(profile, task) + if name in motion.zero_joints] + tags = {view.name: {tag.role: tag for tag in view.tags} for view in profile.vision.views} + frames = {(name, spec.view): {} for name, spec in streams} + count = profile.command.command_count + for row in records: + if (row.get("kind") != "diagnostic_observation_frame" + or row.get("sample_phase") != "joint_zero" + or (row.get("task_name"), row.get("session_epoch"), row.get("motion_version")) + != (motion.task_key, session_epoch, motion_version) + or tuple(row.get("zero_joints", ())) != motion.zero_joints + or row.get("command_unit") != profile.command.unit): + continue + stamp = row.get("image_stamp_ns") + command, feedback = row.get("command_vector"), row.get("feedback_vector") + if (not isinstance(stamp, int) or isinstance(stamp, bool) or stamp <= 0 + or not _complete_vector(command, count) or not _complete_vector(feedback, count) + or any(abs(a-b) > 1e-9 for a, b in zip(command, motion.target))): + continue + for name, spec in streams: + if row.get("view") != spec.view: + continue + roles = [role for role in (spec.parent_role, spec.child_role) + if not tags[spec.view][role].fixed_reference] + complete = bool(roles) + for role in roles: + item = row.get("tags", {}).get(role, {}) + points = np.asarray(item.get("corners_xy"), dtype=float) + complete &= (item.get("tag_role") == role + and item.get("tag_id") == tags[spec.view][role].tag_id + and item.get("candidate_diagnostics", {}).get("observation_stamp_ns") == stamp + and points.shape == (4, 2) and np.all(np.isfinite(points))) + if complete: + frames[name, spec.view][stamp] = row + minimum = profile.acquisition.fixed_reference_minimum_frames + missing = [f"{name}:{view}:{len(items)}/{minimum}" + for (name, view), items in frames.items() if len(items) < minimum] + if not frames or missing: + raise ValueError("diagnostic_joint_zero_observations_missing:" + ",".join(missing)) + selected = {(view, stamp): row for (_, view), items in frames.items() + for stamp, row in sorted(items.items())[-minimum:]} + latest = max(selected.values(), key=lambda row: row["image_stamp_ns"]) + return DiagnosticZeroAttempt(tuple(motion.zero_joints), motion.task_key, reason, + session_epoch, motion_version, tuple(latest["command_vector"]), + tuple(latest["feedback_vector"]), tuple(sorted(selected)), tuple(sorted(frames))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_report.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_report.py new file mode 100644 index 0000000..9289057 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_report.py @@ -0,0 +1,181 @@ +"""Summarize captured pose evidence without fitting or certifying motion.""" + +from collections import defaultdict +import hashlib +import json +import math +from pathlib import Path + +from ..core.geometry.pnp import rotation_distance_rad + + +def pose_step_summary(records): + """Compare adjacent accepted images within one uninterrupted scan unit. + + Multiple joint observations of the same Tag image count once. Cached + and image-verified fixed poses and long gaps are excluded from the step statistic, + and all original evidence remains in the raw journal. + """ + streams = defaultdict(dict) + for row in records: + if row.get("sample_phase") != "sweep": + continue + for pose in row.get("pnp_observation_evidence", {}).values(): + if pose["pose_source"] != "current_image": + continue + key = (row["view"], pose["tag_role"], row["task_name"], row["cycle"], + row["direction"], row.get("attempt", 1)) + streams[key][row["image_stamp_ns"]] = pose + tags = {} + for (view, role, task, cycle, direction, attempt), frames in streams.items(): + tag = tags.setdefault(f"{view}:{role}", { + "view": view, "tag_role": role, "tag_id": next(iter(frames.values()))["tag_id"], + "accepted_image_count": 0, "adjacent_pair_count": 0, + "maximum_rotation_step_deg": None, "largest_steps": [], + }) + tag["accepted_image_count"] += len(frames) + ordered = sorted(frames.items()) + steps = [] + for (before, a), (after, b) in zip(ordered, ordered[1:]): + interval = (after-before)/1e9 + if not 0 < interval <= .2: + continue + angle = math.degrees(rotation_distance_rad( + a["selected_pose"]["quaternion_xyzw"], b["selected_pose"]["quaternion_xyzw"])) + steps.append({"rotation_step_deg": angle, "interval_ms": interval*1000, + "from_image": f"{view}:{before}", "to_image": f"{view}:{after}", + "task": task, "cycle": cycle, "direction": direction, "attempt": attempt}) + tag["adjacent_pair_count"] += len(steps) + tag["largest_steps"] = sorted((*tag["largest_steps"], *steps), + key=lambda item: item["rotation_step_deg"], reverse=True)[:5] + if tag["largest_steps"]: + tag["maximum_rotation_step_deg"] = tag["largest_steps"][0]["rotation_step_deg"] + return tags + + +def fixed_reference_summary(records): + """Report image consistency, not fabricated zero angular motion of a base.""" + images = {} + for row in records: + evidence = row.get("tags", {}) if row.get("kind") == "diagnostic_observation_frame" else row.get("pnp_observation_evidence", {}) + items = (evidence,) if row.get("kind") == "pnp_branch_event" else evidence.values() + for item in items: + diagnostics = item.get("candidate_diagnostics", {}) + if diagnostics.get("pose_constraint") != "fixed_reference": + continue + stamp = row["image_stamp_ns"] + if diagnostics.get("observation_stamp_ns") == stamp: + images[(row["view"], item["tag_role"], stamp)] = item + summary = {} + for (view, role, _), item in images.items(): + tag = summary.setdefault(f"{view}:{role}", {"view": view, "tag_role": role, + "tag_id": item["tag_id"], "image_count": 0, "accepted_image_count": 0, + "maximum_reprojection_error_px": None}) + tag["image_count"] += 1 + tag["accepted_image_count"] += int(item.get("selected_pose") is not None) + error = item["candidate_diagnostics"].get("fixed_reference_reprojection_error_px") + if error is not None and math.isfinite(float(error)): + previous = tag["maximum_reprojection_error_px"] + tag["maximum_reprojection_error_px"] = float(error) if previous is None else max(previous, float(error)) + return summary + + +def motion_initialization_summary(records): + """Keep per-joint hypothesis evidence without promoting a local score. + + A relation can fit in one hypothesis while the coupled chain remains + unresolved. Both the local result and that dependency are reported; the + smallest residual is never presented as a confirmed physical branch. + """ + summaries = [] + for row in records: + if row.get("kind") != "motion_branch_initialization": + continue + joints = defaultdict(list) + for hypothesis in row.get("hypotheses", ()): + relations = hypothesis.get("relations", ()) + for relation in relations: + joints[relation["joint"]].append({ + **relation, + "branches": hypothesis["branches"], + "chain_accepted": hypothesis["accepted"], + "other_relations_passed": all(not other["reason"] + for other in relations if other["joint"] != relation["joint"]), + }) + summaries.append({ + "task_name": row["task_name"], "view": row["view"], + "status": row["status"], "reason": row["reason"], + "source_image_count": len(row.get("source_image_stamps", ())), + "joint_hypotheses": dict(joints), + "accuracy_validated": False, + }) + return summaries + + +def _joint_zero_status(profile, joints, references, attempts, records): + """Every attempted zero is either actual frozen evidence or explicit raw-only.""" + if (set(references) & set(attempts) + or set(references) | set(attempts) != joints): + raise ValueError("diagnostic_capture_incomplete_units_or_joint_zeros") + recorded = {json.dumps(row, sort_keys=True, allow_nan=False) + for row in records if row.get("kind") == "diagnostic_joint_zero_attempt"} + statuses = {name: {"status": "confirmed", "raw_only": False} + for name in sorted(references)} + for name, attempt in sorted(attempts.items()): + if (name not in attempt.joint_names + or attempt.task_name != profile.motion.joint_zero_references[name].task_key + or json.dumps(attempt.as_record(), sort_keys=True, allow_nan=False) not in recorded): + raise ValueError("diagnostic_joint_zero_attempt_evidence_missing") + if any(row.get("kind") in {"joint_sample", "secondary_joint_sample", "joint_zero_reference"} + and row.get("joint", row.get("observation_joint")) == name for row in records): + raise ValueError("diagnostic_unresolved_joint_has_accepted_samples") + statuses[name] = {"status": "unresolved", "raw_only": True, + "reason": attempt.reason, "attempt": attempt.as_record()} + return statuses + + +def build_diagnostic_report(profile, plan, *, raw_path: Path, records, references, + completed_units, expected_units, protected_inputs, command, feedback, + diagnostic_zero_attempts=None): + expected = {(unit.task_key, unit.cycle, unit.direction) for unit in expected_units} + joints = {name for task in profile.motion.tasks if task.key in plan.task_keys for name in task.joints} + if set(completed_units) != expected: + raise ValueError("diagnostic_capture_incomplete_units_or_joint_zeros") + attempts = {} if diagnostic_zero_attempts is None else diagnostic_zero_attempts + zero_status = _joint_zero_status(profile, joints, references, attempts, records) + counts = {name: {} for name in sorted(joints)} + phase_counts = {name: {} for name in sorted(joints)} + raw_phase_counts = {} + for row in records: + if row.get("kind") == "joint_sample" and row.get("joint") in joints: + key = f"{row['view']}:{row['direction']}:attempt={row.get('attempt', 1)}" + counts[row["joint"]][key] = counts[row["joint"]].get(key, 0)+1 + phase = phase_counts[row["joint"]].setdefault(row.get("sample_phase", "unknown"), {}) + phase[key] = phase.get(key, 0) + 1 + elif row.get("kind") == "diagnostic_observation_frame" and row.get("task_name") in plan.task_keys: + key = f"{row['view']}:{row['direction']}:attempt={row.get('attempt', 1)}" + phase = raw_phase_counts.setdefault(row.get("sample_phase", "unknown"), {}) + phase[key] = phase.get(key, 0) + 1 + unit_quality = [dict(row) for row in records if row.get("kind") == "scan_unit_complete"] + pose_gaps = [row for row in unit_quality if row.get("accepted_pose_quality", {}).get("passed") is False] + return { + "format": "calibration_diagnostic_capture_v1", "schema_version": 1, + **plan.as_dict(), "result": "DIAGNOSTIC_COMPLETE", "publication_allowed": False, + "accuracy_validated": False, "profile_id": profile.key.profile_id, + "raw_samples_path": str(raw_path.resolve()), + "raw_samples_sha256": hashlib.sha256(raw_path.read_bytes()).hexdigest(), + "protected_inputs": dict(protected_inputs), + "completed_units": sorted(completed_units), "joint_sample_counts": counts, + "joint_sample_counts_by_phase": phase_counts, + "raw_observation_counts_by_phase": raw_phase_counts, + "joint_zero_references": {name: references[name].as_record() for name in sorted(references)}, + "joint_zero_status": zero_status, + "missing_joint_zero_references": sorted(attempts), + "motion_initialization_summary": motion_initialization_summary(records), + "final_command": list(command), "final_feedback": list(feedback), + "pose_step_summary": pose_step_summary(records), + "fixed_reference_summary": fixed_reference_summary(records), + "unit_quality": unit_quality, + "accepted_pose_quality_incomplete_attempts": len(pose_gaps), + "note": "仅完成原始观测诊断采集；未确认零位的关节仅保留原始角点和候选，不生成关节角样本。零位状态另见 joint_zero_status，可信姿态覆盖另见 unit_quality。未进行精度验收，未拟合或发布正式 JSON/URDF。", + } diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py index c64d8b3..53bc4ab 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -9,11 +9,20 @@ from typing import Any, Mapping, Sequence import numpy as np from ..core import CalibrationProfile, TaskSpec, validate_profile +from ..core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from .diagnostic_capture import DiagnosticCapturePlan from ..core.domain.profile import ACQUISITION_POLICY_VERSION TRAINING_CYCLES = (0, 1, 2) HOLDOUT_CYCLE = 3 +CAPTURE_SCHEDULE_VERSION = "unified_schedule_v2_single_pass" +SEPARATE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v3_separate_mapping" + + +def capture_schedule_version(profile): + return (SEPARATE_CAPTURE_SCHEDULE_VERSION if profile.acquisition.command_capture_mode == "separate" + else CAPTURE_SCHEDULE_VERSION) @dataclass(frozen=True) @@ -24,6 +33,7 @@ class ScanUnit: start: float end: float speed: float + sample_phase: str = "sweep" @dataclass(frozen=True) @@ -119,21 +129,33 @@ def evaluate_observability( class CalibrationEngine: """Schedule and post-direction quality policy used by the single session executor.""" - def __init__(self, profile: CalibrationProfile) -> None: + def __init__( + self, profile: CalibrationProfile, *, + diagnostic_capture: DiagnosticCapturePlan | None = None, + ) -> None: validate_profile(profile) self.profile = profile + self.diagnostic_capture = diagnostic_capture @property def checkpoint_token(self) -> str: return ACQUISITION_POLICY_VERSION + @property + def capture_schedule_version(self) -> str: + return capture_schedule_version(self.profile) + def scan_units(self) -> tuple[ScanUnit, ...]: units: list[ScanUnit] = [] + plan = self.diagnostic_capture + cycles = plan.cycles if plan is not None else (*TRAINING_CYCLES, HOLDOUT_CYCLE) for task in self.profile.motion.tasks: + if plan is not None and task.key not in plan.task_keys: + continue speed = self._formal_speed(task) forward = "increasing" if task.end_value > task.start_value else "decreasing" reverse = "decreasing" if forward == "increasing" else "increasing" - for cycle in (*TRAINING_CYCLES, HOLDOUT_CYCLE): + for cycle in cycles: units.extend(( ScanUnit( task.key, cycle, forward, task.start_value, @@ -144,6 +166,13 @@ class CalibrationEngine: task.start_value, speed, ), )) + if plan is None and self.profile.acquisition.command_capture_mode == "separate": + from ..core.domain.sampling import command_training_cycles, command_holdout_cycle + for cycle in (*command_training_cycles(self.profile), command_holdout_cycle(self.profile)): + units.extend(( + ScanUnit(task.key, cycle, forward, task.start_value, task.end_value, speed, "steady"), + ScanUnit(task.key, cycle, reverse, task.end_value, task.start_value, speed, "steady"), + )) return tuple(units) def mapping_probe_delta(self, task: TaskSpec) -> float | None: @@ -201,13 +230,29 @@ class CalibrationEngine: return SweepQuality(False, (*quality.failures, "endpoint_observation"), quality.warnings, quality.metrics) return quality - def resume_compatible(self, session_start: Mapping[str, Any]) -> bool: + def capture_compatible(self, session_start: Mapping[str, Any]) -> bool: + """Known capture schedules share the unchanged measurement contract. + + Historical full-sweep/steady journals have no schedule field. They + can still be evaluated on their own, but cannot be mixed into a new + directional-waypoint session through resume. + """ return ( - session_start.get("acquisition_policy_version") + self.diagnostic_capture is None + and not session_start.get("diagnostic_capture") + and session_start.get("acquisition_policy_version") == ACQUISITION_POLICY_VERSION + and session_start.get("capture_schedule_version") in ( + (capture_schedule_version(self.profile),) if self.profile.acquisition.command_capture_mode == "separate" + else (None, CAPTURE_SCHEDULE_VERSION)) + and session_start.get("pose_tracking_policy_version") + == POSE_TRACKING_POLICY_VERSION and session_start.get("profile_id") == self.profile.key.profile_id ) + def resume_compatible(self, session_start: Mapping[str, Any]) -> bool: + return (self.capture_compatible(session_start) + and session_start.get("capture_schedule_version") == capture_schedule_version(self.profile)) def _formal_speed(self, task: TaskSpec) -> float: @@ -222,6 +267,6 @@ class CalibrationEngine: __all__ = [ - "ACQUISITION_POLICY_VERSION", "CalibrationEngine", + "ACQUISITION_POLICY_VERSION", "CAPTURE_SCHEDULE_VERSION", "CalibrationEngine", "HOLDOUT_CYCLE", "ScanUnit", "SweepQuality", "TRAINING_CYCLES", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py index e4be97d..25ac0cc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py @@ -4,9 +4,13 @@ No ROS dependency, hand-identity branch, alternate retry loop or serializer. The same driver is usable with real SDK transport and a virtual clock. """ +from __future__ import annotations + from dataclasses import replace import math +from .diagnostic_capture import DiagnosticCapturePlan +from .diagnostic_reference import DiagnosticZeroAttempt from .session import CalibrationPhase as Phase, CalibrationSession from .motion_execution import MotionCommand from .scan_quality import evaluate_capture_unit @@ -15,16 +19,20 @@ from .trajectory import (build_calibration_preparation_waypoints, class SessionExecution: - def __init__(self, profile): + def __init__(self, profile, *, diagnostic_capture: DiagnosticCapturePlan | None = None): self.profile = profile - self.session = CalibrationSession(profile) + self.diagnostic_capture = diagnostic_capture + self.session = CalibrationSession(profile, diagnostic_capture=diagnostic_capture) self.first_cycle_spans = {} self.completed_units = set() self.last_quality = None + self.last_diagnostic_quality = None self._effects = [] self._action_key = None self._entered_task = None self.zero_references = {} + self.sampling_plans = {} + self.diagnostic_zero_attempts: dict[str, DiagnosticZeroAttempt] = {} @property def action(self): @@ -72,13 +80,15 @@ class SessionExecution: current = tuple(pose) if self.profile.artifacts.output_schema_version >= 3: pending = {name: spec for name, spec in self.profile.motion.joint_zero_references.items() - if spec.task_key == task.key and name not in self.zero_references} + if spec.task_key == task.key and name not in self.zero_references + and not (self.diagnostic_capture is not None and name in self.diagnostic_zero_attempts)} groups = {} for name, spec in pending.items(): groups.setdefault((spec.command, spec.approach_commands), []).append(name) for (target, approach), names in groups.items(): for pose in (*approach, target): - entry.extend(MotionCommand("zero_approach", point, unit.speed, **common) + entry.extend(MotionCommand("zero_approach", point, unit.speed, + reference_joints=tuple(sorted(names)), **common) for point in build_joint_reference_waypoints(task, pose, profile=self.profile, current_command=current)) current = pose @@ -99,22 +109,58 @@ class SessionExecution: return (MotionCommand("mapping_probe", tuple(target), speed, **common), MotionCommand("mapping_return", start, speed, **common)) if action.phase in {Phase.SWEEP, Phase.RESCAN}: - target = tuple(build_calibration_motion_command(task, unit.end, profile=self.profile)) + if self.diagnostic_capture is None and self.profile.acquisition.command_capture_mode == "separate": + if unit.sample_phase == "sweep": + return (MotionCommand("sweep", tuple(build_calibration_motion_command(task, unit.end, + profile=self.profile)), unit.speed, direction=unit.direction, **common),) + from .steady import steady_targets + plan = self.sampling_plans.get(task.key) + nodes = tuple(plan["training_nodes"]) if plan is not None else None + metadata = {} if plan is None else {"steady_training_nodes": nodes, + "sampling_plan_sha256": plan["plan_sha256"]} + # One move-and-hold effect per mapping point: arrival and the + # stable image window share the same feedback history. + return tuple(MotionCommand("steady", tuple(build_calibration_motion_command(task, value, + profile=self.profile)), unit.speed, direction=unit.direction, steady_index=index, **common, **metadata) + for index, value in enumerate(steady_targets(self.profile, unit, training_nodes=nodes))) + if self.diagnostic_capture is not None: + target = tuple(build_calibration_motion_command(task, unit.end, profile=self.profile)) + sweep = MotionCommand("sweep", target, unit.speed, direction=unit.direction, **common) + if not self.diagnostic_capture.hold_command_values: + return (sweep,) + # Compare the same declared commands after arrival from both + # endpoints. Keep the ordinary safety/trajectory driver and + # retain the complete hold for time-dependent diagnostics. + start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) + values = sorted(self.diagnostic_capture.hold_command_values, + reverse=unit.direction == "decreasing") + return (sweep, MotionCommand("steady_prepare", start, unit.speed, **common), + *(MotionCommand("steady", + tuple(build_calibration_motion_command(task, value, profile=self.profile)), + unit.speed, direction=unit.direction, steady_index=index, + minimum_hold_seconds=self.diagnostic_capture.hold_seconds, **common) + for index, value in enumerate(values))) from .steady import steady_targets - start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) - effects = [MotionCommand("sweep", target, unit.speed, direction=unit.direction, **common), - MotionCommand("steady_prepare", start, unit.speed, **common)] - effects.extend(MotionCommand("steady", - tuple(build_calibration_motion_command(task, value, profile=self.profile)), - unit.speed, direction=unit.direction, steady_index=index, **common) - for index, value in enumerate(steady_targets(self.profile, unit))) + effects = [] + # Traverse the direction once. Separate motion effects preserve + # the original sweep/steady image identities and capture gates; + # a held frame is never copied into the moving sample set. + for index, value in enumerate(steady_targets(self.profile, unit)): + point = tuple(build_calibration_motion_command(task, value, profile=self.profile)) + if index: + effects.append(MotionCommand("sweep", point, unit.speed, + direction=unit.direction, **common)) + effects.append(MotionCommand("steady", point, unit.speed, + direction=unit.direction, steady_index=index, **common)) return tuple(effects) return () def motion_complete(self): if not self._effects: raise RuntimeError("cannot complete a motion that was not requested") - if any(name not in self.zero_references for name in self._effects[0].zero_joints): + if any(name not in self.zero_references and not ( + self.diagnostic_capture is not None and name in self.diagnostic_zero_attempts) + for name in self._effects[0].zero_joints): raise RuntimeError("joint zero acquisition cannot complete without frozen evidence") self._effects.pop(0) if self._effects: @@ -135,12 +181,48 @@ class SessionExecution: else: raise RuntimeError(f"unexpected completed motion:{phase.value}") + def repeat_zero_approach(self, current): + """Replace only the current unreferenced preparation, preserving the scan.""" + motion = self._effects[0] if self._effects else None + if (self.session.phase != Phase.PREPARE or motion is None or motion.phase != "joint_zero" + or set(motion.zero_joints) & self.zero_references.keys()): + raise ValueError("zero_recovery_outside_unreferenced_preparation") + task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + specs = [self.profile.motion.joint_zero_references[name] for name in motion.zero_joints] + if any(spec != specs[0] for spec in specs): + raise ValueError("zero_recovery_incompatible_group") + effects = [] + for target in (*specs[0].approach_commands, specs[0].command): + effects.extend(replace(motion, phase="zero_approach", target=point, + zero_joints=(), reference_joints=motion.zero_joints) + for point in build_joint_reference_waypoints(task, target, profile=self.profile, + current_command=current)) + current = target + self._effects[:1] = [*effects, motion] + + def record_diagnostic_zero_attempt(self, attempt: DiagnosticZeroAttempt): + """A diagnostic observation closes an attempt, never creates a zero.""" + motion = self._effects[0] if self._effects else None + if (self.diagnostic_capture is None or motion is None or motion.phase != "joint_zero" + or (attempt.task_name, attempt.joint_names) != (motion.task_key, motion.zero_joints)): + raise ValueError("diagnostic_joint_zero_attempt_outside_current_motion") + if any(name in self.zero_references or name in self.diagnostic_zero_attempts + for name in attempt.joint_names): + raise ValueError("diagnostic_joint_zero_attempt_replacement_forbidden") + self.diagnostic_zero_attempts.update({name: attempt for name in attempt.joint_names}) + def evaluate(self, records): if self.session.phase != Phase.EVALUATE: raise RuntimeError("sweep evaluation outside EVALUATE") action = self.action - quality = evaluate_capture_unit(self.profile, action.scan_unit, action.retry+1, - records, first_cycle_spans=self.first_cycle_spans) + if self.diagnostic_capture is None: + quality = evaluate_capture_unit(self.profile, action.scan_unit, action.retry+1, + records, first_cycle_spans=self.first_cycle_spans) + else: + from .diagnostic_quality import evaluate_diagnostic_unit + self.last_diagnostic_quality = evaluate_diagnostic_unit(self.profile, + action.scan_unit, action.retry+1, records) + quality = self.last_diagnostic_quality.observations self.last_quality = quality if quality.passed: unit = action.scan_unit @@ -150,6 +232,8 @@ class SessionExecution: def restore(self, decision, records): """Recheck each durable unit using the current, hash-bound policy.""" + if self.diagnostic_capture is not None: + raise ValueError("diagnostic capture cannot restore previous data") self.first_cycle_spans.clear() completed = set(decision.completed_units) if decision.reuse else set() valid = [] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/feedback.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/feedback.py new file mode 100644 index 0000000..5c910b6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/feedback.py @@ -0,0 +1,19 @@ +"""Validate the SDK measurement-time contract before accepting any feedback.""" + +import json +import math + + +def decode_timed_feedback(text): + data = json.loads(text) + if not isinstance(data, dict) or data.get("schema") != "can_feedback_v1": + raise ValueError("feedback_timing_schema_invalid") + names, positions, stamps = (data.get(key) for key in ("name", "position", "channel_stamps_ns")) + if (not isinstance(names, list) or not isinstance(positions, list) or not isinstance(stamps, list) + or not positions or len(positions) != len(stamps) or len(names) != len(positions) + or any(not isinstance(name, str) for name in names) + or any(isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value) for value in positions) + or any(type(stamp) is not int or stamp <= 0 for stamp in stamps) + or type(data.get("stamp_ns")) is not int or data["stamp_ns"] != max(stamps)): + raise ValueError("feedback_timing_payload_invalid") + return tuple(names), tuple(positions), data["stamp_ns"], tuple(stamps) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py new file mode 100644 index 0000000..95416d7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py @@ -0,0 +1,33 @@ +"""Keep every camera's measured corners under the same motion/time identity. + +These records do not depend on a PnP branch or a task-local hinge model. +Additional views observe the existing motion; they never request a movement, +declare a joint measured, or change the primary stream's acquisition gates. +""" + +IMAGE_OBSERVATION_POLICY = "all_view_rectified_corners_v1" + + +def image_observation_record(profile, frame, motion): + if motion is None or not motion.recording or motion.phase not in {"sweep", "steady", "joint_zero"}: + return None + tags = next(view.tags for view in profile.vision.views if view.name == frame.view) + return { + "kind": "image_observation_frame", "policy": IMAGE_OBSERVATION_POLICY, + "task_name": motion.task_key, "cycle": motion.cycle, + "direction": motion.direction, "attempt": motion.attempt, + "sample_phase": motion.phase, "steady_index": motion.steady_index, + "view": frame.view, "image_stamp_ns": frame.stamp_ns, + "command_unit": profile.command.unit, + "command_vector": None if frame.command is None else list(frame.command), + "feedback_vector": None if frame.feedback is None else list(frame.feedback), + "command_direction_by_index": list(frame.command_directions), + "state_image_sync_error_ns": None if frame.feedback is None else frame.skew_ns, + "camera_matrix": frame.camera_matrix.tolist(), + "camera_matrix_source": "CameraInfo.P[:3,:3]", "input_is_rectified": True, + "tags": {tag.role: { + "tag_id": tag.tag_id, "tag_size_m": tag.size_m, + "fixed_reference": tag.fixed_reference, + "corners_xy": frame.corners[tag.role].tolist(), + } for tag in tags if tag.role in frame.corners}, + } diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_pairing.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_pairing.py new file mode 100644 index 0000000..340a6b4 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_pairing.py @@ -0,0 +1,27 @@ +"""Bounded exact-stamp pairing; transport arrival order is not image identity.""" + +from collections import OrderedDict + + +class ExactImagePairing: + def __init__(self, capacity=8): + self.capacity = capacity + self.images, self.detections = OrderedDict(), OrderedDict() + self.last_emitted = -1 + + def add(self, kind, stamp, message): + if stamp <= self.last_emitted: + return None + own, other = ((self.images, self.detections) if kind == "image" + else (self.detections, self.images)) + own[stamp] = message + while len(own) > self.capacity: + own.popitem(last=False) + if stamp not in other: + return None + image, detection = self.images.pop(stamp), self.detections.pop(stamp) + self.last_emitted = stamp + for buffer in (self.images, self.detections): + for old in [value for value in buffer if value <= stamp]: + del buffer[old] + return image, detection diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py index c5ea961..0edf454 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py @@ -24,6 +24,7 @@ class TagDetection: corners: tuple[tuple[float, float], ...] hamming: int decision_margin: float + border_quality: Mapping[str, object] | None = None @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py index df38b2d..a005aae 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py @@ -1,5 +1,9 @@ """Stage old data until its individual zero poses have been checked again.""" +from __future__ import annotations + +from dataclasses import dataclass + import numpy as np from scipy.spatial.transform import Rotation @@ -8,12 +12,97 @@ from .scan_quality import evaluate_capture_unit from .resume import ResumeVerifier, TagPoseFingerprint +@dataclass +class TaskResumeImport: + """Bounded journal transfer while live feedback and safety keep running.""" + + task_key: str + units: tuple + rows: tuple + offset: int = 0 + + def take_batch(self): + batch = self.rows[self.offset:self.offset + 64] + self.offset += len(batch) + return batch + + @property + def complete(self): + return self.offset == len(self.rows) + + +@dataclass(frozen=True) +class PreparedResume: + """Static checkpoint checks completed before live callbacks or motion. + + This grants no reuse: the coordinator must still compare the current + fixed reference and verify each joint's physical zero installation. + """ + + header: dict + fixed_reference: dict + rows: tuple + completed_units: frozenset + joints: JointResume + + @classmethod + def load(cls, profile, engine, path, serial_number): + from .acquisition import load_capture + from .diagnostic_capture import reject_diagnostic_capture + + rows = tuple(load_capture(path)) + reject_diagnostic_capture(rows) + headers = [r for r in rows if r.get("kind") == "session_start"] + references = [r for r in rows if r.get("kind") == "fixed_base_reference_locked"] + if len(headers) != 1 or len(references) != 1: + raise ValueError("missing unique checkpoint header/reference") + header = headers[0] + if header.get("serial_number") != serial_number or not engine.resume_compatible(header): + raise ValueError("checkpoint identity/policy changed") + complete = frozenset((r["task_name"], r["cycle"], r["direction"]) for r in rows + if r.get("kind") == "scan_unit_complete" and r.get("passed") is True) + joints = JointResume(profile) + if profile.artifacts.output_schema_version >= 3: + joints.stage(engine, rows, complete) + return cls(header, references[0], rows, complete, joints) + + def _pose(samples, field): poses = [getattr(sample, field) for sample in samples] return TagPoseFingerprint(tuple(Rotation.from_quat([p.quaternion_xyzw for p in poses]).mean().as_quat()), tuple(np.median([p.translation_xyz_m for p in poses], axis=0))) +def _index_motion_provenance(rows): + from .motion_provenance import source_frame_sha256 + + zero_images = {(r.get("view"), r.get("image_stamp_ns")) for r in rows + if r.get("kind") == "joint_zero_sample"} + kinds = {"motion_branch_observation", "motion_branch_initialization", + "joint_zero_motion_reference_verified", "joint_zero_sample"} + return {source_frame_sha256(row): row for row in rows + if row.get("kind") in kinds or (row.get("kind") == "pnp_candidate_frame" + and (row.get("view"), row.get("image_stamp_ns")) in zero_images)} + + +def reference_pose_failures(previous, current): + """The same read-back check authorizes live and persisted reference reuse.""" + failures = [] + if (previous.joint != current.joint or previous.task_key != current.task_key + or previous.channel != current.channel or previous.baseline_command != current.baseline_command): + failures.append("reference_contract") + verifier = ResumeVerifier() + for view in {s.view for s in previous.samples}: + old = [s for s in previous.samples if s.view == view] + new = [s for s in current.samples if s.view == view] + if not new or {s.command for s in old} != {s.command for s in new}: + failures.append(f"held_pose:{view}") + continue + for field in ("parent_pose_common", "child_pose_common"): + verifier._compare_poses(field, {view: _pose(old, field)}, {view: _pose(new, field)}, failures) + return failures + + class JointResume: def __init__(self, profile): self.profile = profile @@ -22,9 +111,19 @@ class JointResume: self.units = set() self.rows = () self.first_cycle_spans = {} + self._motion_provenance_pending = {} + self._motion_provenance_source = None + self._motion_provenance_emitted = set() def stage(self, engine, rows, units): + headers = [row for row in rows if row.get("kind") == "session_start"] + if len(headers) != 1 or not engine.resume_compatible(headers[0]): + raise ValueError("joint_resume_checkpoint_policy_incompatible") references = read_joint_zero_references(self.profile, rows) + from .motion_provenance import validate_motion_provenance + validate_motion_provenance(self.profile, rows, references=references) + from ..core.fitting.command_sampling import validate_sampling_plans + validate_sampling_plans(self.profile, rows) spans, valid = {}, set() for unit in engine.scan_units(): key = (unit.task_key, unit.cycle, unit.direction) @@ -38,6 +137,9 @@ class JointResume: valid.add(key) self.references, self.units, self.rows = references, valid, tuple(rows) self.first_cycle_spans = spans + self._motion_provenance_source = _index_motion_provenance(self.rows) + self._motion_provenance_pending.clear() + self._motion_provenance_emitted.clear() def spans_for_task(self, task_key): from .scan_quality import observation_streams @@ -50,23 +152,45 @@ class JointResume: previous = self.references.get(reference.joint) if previous is None: return reference - failures = [] - if previous.task_key != reference.task_key or previous.channel != reference.channel: - failures.append("reference_contract") - verifier = ResumeVerifier() - for view in {s.view for s in previous.samples}: - old = [s for s in previous.samples if s.view == view] - new = [s for s in reference.samples if s.view == view] - if not new or {s.command for s in old} != {s.command for s in new}: - failures.append(f"held_pose:{view}") - continue - for field in ("parent_pose_common", "child_pose_common"): - verifier._compare_poses(field, {view: _pose(old, field)}, {view: _pose(new, field)}, failures) + failures = reference_pose_failures(previous, reference) + if previous.branch_references and not reference.branch_references: + failures.append("motion_evidence_missing") if failures: raise ValueError(f"joint_resume_reference_changed:{reference.joint}:{','.join(failures)}; 旧数据不能与新零位拼接，请重新开始") self.verified.add(reference.joint) + if previous.branch_references: + if self._motion_provenance_source is None: + self._motion_provenance_source = _index_motion_provenance(self.rows) + zero_images = {(row.get("view"), row.get("image_stamp_ns")) + for row in self._motion_provenance_source.values() + if row.get("kind") == "joint_zero_sample" and row.get("joint") == reference.joint} + for identity, row in self._motion_provenance_source.items(): + if (row["kind"] in {"joint_zero_sample", "joint_zero_motion_reference_verified"} + and row.get("joint") != reference.joint): + continue # An interrupted resume must not import unverified joints' references. + if (row["kind"] == "pnp_candidate_frame" + and (row.get("view"), row.get("image_stamp_ns")) not in zero_images): + continue + if identity not in self._motion_provenance_emitted: + self._motion_provenance_pending[identity] = row + self._motion_provenance_pending[f"verified:{reference.joint}"] = { + "kind": "joint_zero_motion_reference_verified", "joint": reference.joint, + "current_reference": reference.as_record(), "preserved_reference": previous.as_record(), + } return previous # Preserve the original measured zero, including its identity. + def take_motion_provenance_records(self): + """Drain only after the caller atomically freezes all verified roles.""" + from .motion_provenance import source_frame_sha256 + rows = [] + for key, row in self._motion_provenance_pending.items(): + identity = source_frame_sha256(row) if key.startswith("verified:") else key + if identity not in self._motion_provenance_emitted: + rows.append(row) + self._motion_provenance_emitted.add(identity) + self._motion_provenance_pending.clear() + return tuple(rows) + def take_task(self, task_key): task = next(t for t in self.profile.motion.tasks if t.key == task_key) if not set(task.joints) <= self.verified: @@ -74,5 +198,8 @@ class JointResume: units = {key for key in self.units if key[0] == task_key} rows = tuple(r for r in self.rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in units and r.get("kind") not in {"joint_zero_reference", "joint_zero_sample"}) + if units: + rows = tuple(r for r in self.rows if r.get("kind") == "command_sampling_plan" + and r.get("task_name") == task_key) + rows self.units.difference_update(units) return tuple(sorted(units)), rows diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index 1e971ed..067fd98 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -26,6 +26,10 @@ class MotionCommand: attempt: int = 1 steady_index: int | None = None zero_joints: tuple[str, ...] = () + reference_joints: tuple[str, ...] = () + minimum_hold_seconds: float = 0.0 + steady_training_nodes: tuple[float, ...] = () + sampling_plan_sha256: str = "" @property def recording(self): @@ -36,6 +40,8 @@ class MotionExecution: def __init__(self, profile, command: MotionCommand, *, initial_command, initial_feedback, now: float, identity: str): self.profile, self.command, self.identity = profile, command, str(identity) + if not math.isfinite(command.minimum_hold_seconds) or command.minimum_hold_seconds < 0: + raise ValueError("minimum hold must be finite and nonnegative") self.start = tuple(float(v) for v in initial_command) self.start_feedback = tuple(float(v) for v in initial_feedback) self.started_at = float(now) @@ -113,7 +119,8 @@ class MotionExecution: # Data sufficiency is evaluated after the direction; never wait # indefinitely for a Tag or exact command/feedback equality. return (self.finished_at is not None and - now-self.finished_at >= self.profile.acquisition.steady_timeout_seconds) + now-self.finished_at >= max(self.profile.acquisition.steady_timeout_seconds, + self.command.minimum_hold_seconds)) if self.phase < 1 or self.finished_at is None or len(self.history) < 3: return False hold = float(self.profile.motion.speed_parameters.get("endpoint_hold_seconds", 0.25)) @@ -127,13 +134,29 @@ class MotionExecution: return False return True + def minimum_hold_complete(self, now): + return self.finished_at is not None and now-self.finished_at >= self.command.minimum_hold_seconds + def steady_ready(self, now): if self.command.phase not in {"steady", "joint_zero"} or self.finished_at is None: return False window = self.profile.acquisition.steady_window_seconds rows = [(stamp, value) for stamp, value in self.stamped_history - if stamp >= max(self.finished_at, now-window-0.05)] - if len(rows) < 3 or rows[-1][0]-rows[0][0] < window: + if stamp >= self.finished_at] + if not rows or now-rows[-1][0] > window: + return False + # Use the latest independent feedback as the window end, so timer + # ticks between feedback messages cannot turn stable data unstable. + # Keep the sample bracketing its start: a fixed 50 ms margin loses it + # under scheduler jitter. Add the duration to avoid cancellation when + # subtracting large monotonic timestamps at the exact window edge. + end = rows[-1][0] + boundary = next((i for i in range(len(rows)-1, -1, -1) + if rows[i][0]+window <= end), None) + if boundary is None: + return False + rows = rows[boundary:] + if len(rows) < 3: return False indices = range(self.profile.command.command_count) if self.command.phase == "joint_zero" else (self.command.command_index,) return all(max(v[i] for _, v in rows)-min(v[i] for _, v in rows) <= max(2*self.resolution, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py new file mode 100644 index 0000000..bf9783c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py @@ -0,0 +1,272 @@ +"""Read-back of pre-zero geometric branch evidence, without fitting new poses. + +Legacy journals remain readable with their original meaning. A motion-aware +journal cannot lose its header and silently fall back to legacy confirmation. +""" + +import hashlib +import json +import re +from collections.abc import Mapping + +from ..core.domain.reference import joint_zero_moving_roles, read_joint_zero_references +from ..core.geometry.image_motion_replay import ( + IMAGE_POSE_SOURCE, frozen_image_model, has_image_model, replay_image_models, + validate_image_sample, +) +from ..core.geometry.tag_pose.parameters import POSE_TRACKING_POLICY_VERSION + + +MOTION_BRANCH_POLICY_VERSION = POSE_TRACKING_POLICY_VERSION +MOTION_BRANCH_POLICY_VERSIONS = { + "confirmed_branch_v4_motion_geometry", "confirmed_branch_v5_image_motion", + "confirmed_branch_v6_source_geometry", "confirmed_branch_v7_final_image_sampling", + MOTION_BRANCH_POLICY_VERSION, +} + + +def uses_motion_evidence(records): + return any( + row.get("kind") in {"motion_branch_initialization", "motion_branch_observation", + "joint_zero_motion_reference_verified"} + or row.get("pose_tracking_policy_version") in MOTION_BRANCH_POLICY_VERSIONS + or bool(row.get("branch_references")) + or row.get("pnp_observation_evidence", {}).get("candidate_diagnostics", {}).get("motion_evidence_verified") is True + or any(isinstance(item, Mapping) and item.get("candidate_diagnostics", {}).get("motion_evidence_verified") is True + for item in row.get("pnp_observation_evidence", {}).values()) + for row in records) + + +def _fail(reason): + raise ValueError(f"motion_branch_provenance_{reason}") + + +def _canonical(value): + return json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False) + + +def source_frame_sha256(record): + """Bind the entire captured frame, including corners, candidates and SDK state.""" + return hashlib.sha256(_canonical(record).encode()).hexdigest() + + +def _check_source_hashes(payload, observations, hashes): + stamps = payload.get("source_image_stamps", ()) + declared = payload.get("source_frame_hashes") + if (not isinstance(stamps, (tuple, list)) or not stamps + or any(type(stamp) is not int or stamp < 1 for stamp in stamps) + or list(stamps) != sorted(set(stamps)) + or not isinstance(declared, Mapping) or set(declared) != {str(stamp) for stamp in stamps} + or any(not isinstance(value, str) or re.fullmatch(r"[0-9a-f]{64}", value) is None + for value in declared.values())): + _fail("source_hash_coverage_invalid") + for stamp in stamps: + key = (payload["view"], stamp) + source = observations.get(key) + if source is None or any(source.get(field) != payload[field] for field in ( + "task_name", "zero_joints", "session_epoch", "motion_version")): + _fail("source_image_missing_or_changed") + if key not in hashes: + hashes[key] = source_frame_sha256(source) + if hashes[key] != declared[str(stamp)]: + _fail("source_content_changed") + + +def _initializations(records): + observations, evidence, resolved = {}, {}, [] + for row in records: + if row.get("kind") == "motion_branch_observation": + key = (row.get("view"), row.get("image_stamp_ns")) + if key in observations and _canonical(observations[key]) != _canonical(row): + _fail("source_image_changed") + observations[key] = row + if row.get("kind") == "motion_branch_initialization" and row.get("status") == "resolved": + resolved.append(row) + for role, payload in row.get("evidence_payloads", {}).items(): + identity = row.get("evidence_ids", {}).get(role) + if (not isinstance(identity, str) or re.fullmatch(r"[0-9a-f]{64}", identity) is None + or hashlib.sha256(_canonical(payload).encode()).hexdigest() != identity): + _fail("initialization_hash_invalid") + if payload.get("tag_role") != role or any(payload.get(key) != row.get(key) for key in ( + "task_name", "view", "zero_joints", "session_epoch", "motion_version", + "source_image_stamps", "source_frame_hashes", "constraints", + "frozen_image_model", "image_model_sha256")): + _fail("initialization_binding_changed") + stamps = payload.get("source_image_stamps", ()) + if (not stamps or any(type(stamp) is not int or stamp < 1 for stamp in stamps) + or list(stamps) != sorted(set(stamps)) + or not (payload.get("constraints") or payload.get("frozen_image_model")) + or any(type(payload.get(key)) is not int or payload[key] < 1 + for key in ("session_epoch", "motion_version"))): + _fail("initialization_support_invalid") + if identity in evidence and _canonical(evidence[identity]) != _canonical(payload): + _fail("initialization_identity_reused") + evidence[identity] = payload + source_hashes = {} + for row in resolved: + _check_source_hashes(row, observations, source_hashes) + for role, identity in row.get("evidence_ids", {}).items(): + payload = evidence.get(identity) + if payload is None or (payload["view"], payload["tag_role"]) != (row.get("view"), role): + _fail("initialization_role_unbound") + for payload in evidence.values(): + _check_source_hashes(payload, observations, source_hashes) + if has_image_model(payload): + _check_image_initialization(payload, observations) + model = frozen_image_model(payload) + source_hash = getattr(model, "source_urdf_sha256", "") + if source_hash: + headers = [row for row in records if row.get("kind") == "session_start"] + if len(headers) != 1 or headers[0].get("source_urdf_sha256") != source_hash: + _fail("image_model_source_urdf_changed") + return evidence + + +def validate_source_geometry(profile, source_urdf, records): + """Re-derive source constraints before finalization; never trust labels alone.""" + from ..core.geometry.tag_pose.cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY + from ..core.urdf.kinematics import UrdfKinematicModel + from ..profiles.observations import compile_parallel_axis_geometry + + source_model = None + for row in records: + if row.get("kind") != "motion_branch_initialization" or row.get("status") != "resolved": + continue + payload = row.get("frozen_image_model", {}) + if payload.get("policy") != CAD_IMAGE_MOTION_MODEL_POLICY: + continue + model = frozen_image_model(row) + if source_model is None: + source_model = UrdfKinematicModel(source_urdf) + source_hash = hashlib.sha256(source_model.source.read_bytes()).hexdigest() + if (model.source_urdf_sha256 != source_hash + or model.constraints != compile_parallel_axis_geometry(profile, source_model, model.relations)): + _fail("image_model_source_geometry_changed") + + +def _check_image_initialization(payload, observations): + model = frozen_image_model(payload) + if (list(model.source_stamps_ns) != payload["source_image_stamps"] + or len(model.source_stamps_ns) < 24 + or min(len(model.training_stamps_ns), len(model.validation_stamps_ns)) < 12 + or payload["tag_role"] not in {item.relation.child_role for item in model.geometry} + or not {item.relation.joint for item in model.geometry} <= set(payload["zero_joints"])): + _fail("image_model_support_or_roles_changed") + for stamp in model.source_stamps_ns: + source = observations[(payload["view"], stamp)] + if (source.get("cycle") == 3 or source.get("sample_phase") != "zero_approach" + or source.get("input_is_rectified") is not True + or _canonical(source.get("camera_matrix")) != _canonical(model.camera_matrix)): + _fail("image_model_support_not_pre_zero") + for role, size in model.tag_sizes: + if source.get("tags", {}).get(role, {}).get("tag_size_m") != size: + _fail("image_model_tag_size_changed") + + +def _image_frames(records): + frames = {} + for row in records: + if row.get("kind") != "pnp_candidate_frame": + continue + key = (row.get("view"), row.get("image_stamp_ns")) + if key in frames and _canonical(frames[key]) != _canonical(row): + _fail("image_frame_identity_reused") + frames[key] = row + return frames + + +def _reference_authorizations(profile, reference, evidence): + roles = {(item.view, item.tag_role) for item in reference.branch_references} + if roles != joint_zero_moving_roles(profile, reference.joint): + _fail(f"zero_evidence_missing:{reference.joint}") + result = {} + for item in reference.branch_references: + payload = evidence.get(item.motion_evidence_id) + if payload is None or (payload["view"], payload["tag_role"]) != (item.view, item.tag_role): + _fail(f"zero_evidence_unbound:{reference.joint}") + first_zero = min(sample.image_stamp_ns for sample in reference.samples if sample.view == item.view) + if max(payload["source_image_stamps"]) >= first_zero: + _fail(f"initialization_not_before_zero:{reference.joint}") + result[(item.view, item.tag_role, item.motion_evidence_id)] = (item.branch_revision, first_zero) + return result + + +def validate_motion_provenance(profile, records, *, required=False, require_complete=False, references=None): + """Check capture authorization, never certify physical accuracy. + + The explicit ``required`` flag is set by the production owner. Independent + record markers preserve this requirement after a header is removed. + """ + if not required and not uses_motion_evidence(records): + return {"status": "legacy_image_confirmation", "motion_evidence_verified": False} + references = read_joint_zero_references(profile, records) if references is None else references + expected = {name for task in profile.motion.tasks for name in task.joints} + if not references or (require_complete and not expected <= references.keys()): + _fail("joint_zero_references_missing") + evidence = _initializations(records) + image_frames, replayed = _image_frames(records), {} + authorizations = {name: _reference_authorizations(profile, reference, evidence) + for name, reference in references.items()} + zero_images = {(name, sample.view, sample.image_stamp_ns) + for name, reference in references.items() for sample in reference.samples} + for row in records: + if row.get("kind") != "joint_zero_motion_reference_verified": + continue + from .joint_resume import reference_pose_failures + original = read_joint_zero_references(profile, [row["preserved_reference"]]) + current = read_joint_zero_references(profile, [row["current_reference"]]) + name = row.get("joint") + if set(original) != {name} or set(current) != {name} or name not in references: + _fail("resume_reference_binding_invalid") + if (_canonical(original[name].as_record()) != _canonical(references[name].as_record()) + or reference_pose_failures(original[name], current[name])): + _fail(f"resume_reference_changed:{name}") + authorizations[name].update(_reference_authorizations(profile, current[name], evidence)) + zero_images.update((name, sample.view, sample.image_stamp_ns) for sample in current[name].samples) + cycles = {} + for row in records: + kind = row.get("kind") + if kind not in {"joint_sample", "secondary_joint_sample", "joint_zero_sample"}: + continue + name, view, stamp = row.get("joint", row.get("observation_joint")), row.get("view"), row.get("image_stamp_ns") + is_zero = kind == "joint_zero_sample" or row.get("sample_phase") == "joint_zero" + if is_zero and (name, view, stamp) not in zero_images: + continue # Earlier unsettled zero images are retained diagnostics. + if name not in authorizations: + _fail(f"sample_zero_missing:{name}") + roles = {role for role_view, role in joint_zero_moving_roles(profile, name) if role_view == view} + observed = row.get("pnp_observation_evidence", {}) + by_role = {item.get("tag_role"): item for item in observed.values()} + if not roles or not roles <= by_role.keys(): + _fail(f"sample_role_missing:{name}") + for role in roles: + item = by_role[role] + diagnostics = item.get("candidate_diagnostics", {}) + key = (view, role, diagnostics.get("motion_evidence_id")) + authorization = authorizations[name].get(key) + if (authorization is None or item.get("pose_source") not in {"current_image", IMAGE_POSE_SOURCE} + or diagnostics.get("motion_evidence_verified") is not True + or diagnostics.get("branch_revision") != authorization[0] + or diagnostics.get("observation_stamp_ns") != stamp + or diagnostics.get("branch_status") != "tracking" + or (not is_zero and (diagnostics.get("branch_frozen") is not True or stamp <= authorization[1]))): + _fail(f"sample_not_authorized:{name}:{view}:{role}") + payload = evidence[diagnostics["motion_evidence_id"]] + if has_image_model(payload) or has_image_model(item): + if (item.get("pose_source") != IMAGE_POSE_SOURCE + or not has_image_model(payload) + or item.get("image_model_sha256") != payload.get("image_model_sha256")): + _fail(f"sample_image_model_changed:{name}:{view}:{role}") + frame_key = (view, stamp) + frame = image_frames.get(frame_key) + if frame is None: + _fail(f"sample_image_frame_missing:{name}:{view}:{role}") + if frame_key not in replayed: + replayed[frame_key] = replay_image_models(frame) + validate_image_sample(row, frame, replayed[frame_key]) + if not is_zero: + cycles.setdefault(name, set()).add(row.get("cycle")) + if require_complete and any(not {0, 1, 2, 3} <= cycles.get(name, set()) for name in expected): + _fail("independent_cycles_missing") + return {"status": "motion_evidence_verified", "motion_evidence_verified": True, + "joint_count": len(references), "evidence_count": len(evidence), "is_accuracy_certificate": False} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_scope.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_scope.py new file mode 100644 index 0000000..42386a4 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_scope.py @@ -0,0 +1,36 @@ +"""Resolve the immutable observation scope of one complete capture session. + +Formal calibration keeps the Profile's full reference contract. A diagnostic +subset needs every primary and secondary view of its selected tasks, including +tasks scheduled later. This never changes as individual scan units advance. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from ..core.domain.profile import CalibrationProfile + +if TYPE_CHECKING: + from .diagnostic_capture import DiagnosticCapturePlan + + +def required_observation_views( + profile: CalibrationProfile, + diagnostic_capture: DiagnosticCapturePlan | None = None, +) -> tuple[str, ...]: + declared = profile.vision.view_names + if diagnostic_capture is None: + return declared + keys = set(diagnostic_capture.task_keys) + tasks = {task.key: task for task in profile.motion.tasks} + if not keys or not keys <= tasks.keys(): + raise ValueError("diagnostic_observation_tasks_invalid") + measurements = profile.measurement + names = {name for key in keys for name in tasks[key].joints} + names.update(measurements.cross_view_sources[name] + for name in tuple(names) if name in measurements.cross_view_sources) + views = {measurements.measurements[name].view for name in names} + if not views or not views <= set(declared): + raise ValueError("diagnostic_observation_views_invalid") + return tuple(view for view in declared if view in views) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py new file mode 100644 index 0000000..2957973 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py @@ -0,0 +1,113 @@ +"""Describe recorded image and synchronized-observation gaps without judging them.""" + +from collections.abc import Mapping, Sequence +import math +from numbers import Real + +from ..core.domain.profile import CalibrationProfile +from .engine import ScanUnit +from .scan_quality import observation_streams + + +def _finite_number(value: object) -> bool: + return isinstance(value, Real) and not isinstance(value, bool) and math.isfinite(value) + + +def _complete_vector(values: object, count: int) -> bool: + return (isinstance(values, (list, tuple)) and len(values) == count + and all(_finite_number(value) for value in values)) + + +def _current_corners(item: object, role: str, tag_id: int, stamp_ns: int) -> bool: + if not isinstance(item, Mapping): + return False + diagnostics = item.get("candidate_diagnostics") + if (item.get("tag_role") != role or item.get("tag_id") != tag_id + or not isinstance(diagnostics, Mapping) + or diagnostics.get("observation_stamp_ns") != stamp_ns): + return False + corners = item.get("corners_xy") + return (isinstance(corners, (list, tuple)) and len(corners) == 4 + and all(isinstance(point, (list, tuple)) and len(point) == 2 + and all(_finite_number(value) for value in point) for point in corners)) + + +def _maximum_gap_seconds(stamps: Sequence[int]) -> float | None: + ordered = sorted(set(stamps)) + if len(ordered) < 2: + return None + return max(right - left for left, right in zip(ordered, ordered[1:])) / 1e9 + + +def summarize_observation_timing( + profile: CalibrationProfile, + unit: ScanUnit, + attempt: int, + records: Sequence[Mapping], +) -> dict[str, dict[str, int | float | None]]: + """Count raw diagnostic images separately for each required observation stream. + + Only explicit raw image records supply the denominator; accepted joint + samples cannot establish how many images were received. A duplicate image + uses its first record, so separate duplicates cannot manufacture a complete + Tag pair or synchronized command/feedback vectors. + + Visibility requires current corners for every required moving Tag. A + synchronized observation additionally needs complete finite command and + feedback vectors in the Profile's unit. Missing-vector counts cover all raw + images, including those without visible Tags. Gaps use image timestamps; + fewer than two images yield None, and unknown sweep boundaries are not + inferred. These facts neither apply thresholds nor imply accuracy. + """ + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + streams = tuple(observation_streams(profile, task)) + required_views = {spec.view for _, _, spec in streams} + frames: dict[str, dict[int, Mapping]] = {view: {} for view in required_views} + for row in records: + if (row.get("kind") != "diagnostic_observation_frame" + or row.get("sample_phase") != "sweep" + or (row.get("task_name"), row.get("cycle"), row.get("direction"), row.get("attempt")) + != (unit.task_key, unit.cycle, unit.direction, attempt)): + continue + view, stamp = row.get("view"), row.get("image_stamp_ns") + if (view not in frames or not isinstance(stamp, int) + or isinstance(stamp, bool) or stamp <= 0): + continue + frames[view].setdefault(stamp, row) + + tags_by_view = {view.name: {tag.role: tag for tag in view.tags} + for view in profile.vision.views} + result = {} + for field, name, spec in streams: + required = {role: tags_by_view[spec.view][role].tag_id + for role in {spec.parent_role, spec.child_role} + if not tags_by_view[spec.view][role].fixed_reference} + visible_stamps, synchronized_stamps = [], [] + missing_command, missing_feedback = 0, 0 + for stamp, row in frames[spec.view].items(): + correct_unit = row.get("command_unit") == profile.command.unit + command = correct_unit and _complete_vector(row.get("command_vector"), profile.command.command_count) + feedback = correct_unit and _complete_vector(row.get("feedback_vector"), profile.command.command_count) + missing_command += not command + missing_feedback += not feedback + tags = row.get("tags") + visible = (bool(required) and isinstance(tags, Mapping) + and all(_current_corners(tags.get(role), role, tag_id, stamp) + for role, tag_id in required.items())) + if visible: + visible_stamps.append(stamp) + if command and feedback: + synchronized_stamps.append(stamp) + result[f"{field}:{name}:{spec.view}:{unit.direction}"] = { + "raw_frames": len(frames[spec.view]), + "visible_frames": len(visible_stamps), + "synchronized_frames": len(synchronized_stamps), + "missing_command_frames": missing_command, + "missing_feedback_frames": missing_feedback, + "maximum_image_gap_seconds": _maximum_gap_seconds(tuple(frames[spec.view])), + "maximum_synchronized_gap_seconds": _maximum_gap_seconds(synchronized_stamps), + } + return result + + +__all__ = ["summarize_observation_timing"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py index 6a341cb..98a523c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py @@ -2,7 +2,7 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import asdict, dataclass from pathlib import Path from typing import Mapping @@ -39,13 +39,7 @@ class RuntimeParameters: tag_size_m: float detection: DetectionPolicy tracking: PoseTrackingParameters + diagnostic_capture: str = "" def new_trackers(self, views): - return {view: SquareTagPoseTracker( - maximum_reprojection_error_px=self.tracking.maximum_reprojection_error_px, - reprojection_tie_px=self.tracking.reprojection_tie_px, - maximum_pose_jump_rad=self.tracking.maximum_pose_jump_rad, - maximum_translation_jump_m=self.tracking.maximum_translation_jump_m, - maximum_tag_tilt_rad=self.tracking.maximum_tag_tilt_rad, - reset_after_seconds=self.tracking.reset_after_seconds, - ) for view in views} + return {view: SquareTagPoseTracker(**asdict(self.tracking)) for view in views} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py new file mode 100644 index 0000000..726bdf5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py @@ -0,0 +1,128 @@ +"""Diagnostic snapshots of the actual inputs and candidates used by capture. + +Evidence binds accepted images to confirmed, frozen branches; it never changes +the fitting formula or refits a reference. Camera matrices remain in the +session's rectified_camera_model records, indexed by view. +""" + +from collections.abc import Mapping, Sequence +from typing import Protocol + +import numpy as np + +from ..core.domain.profile import TagSpec +from ..core.geometry.pnp import SquareTagPose + + +class PoseDiagnostics(Protocol): + def observation_snapshot(self, role: str, stamp_ns: int) -> tuple[ + tuple[SquareTagPose, ...], dict, int + ] | None: ... + + +def _pose_payload(pose: SquareTagPose) -> dict: + return { + "quaternion_xyzw": list(pose.quaternion_xyzw), + "translation_xyz_m": list(pose.translation_xyz_m), + "reprojection_error_px": float(pose.reprojection_error_px), + } + + +def snapshot_pose_evidence( + tags: Mapping[str, TagSpec], + corners: Mapping[str, np.ndarray], + selected: Mapping[str, SquareTagPose], + tracker: PoseDiagnostics, + cached_roles: Sequence[str], + *, stamp_ns: int, + reference_revisions: Mapping[str, int] | None = None, +) -> dict[str, dict]: + evidence = {} + for role in sorted(selected.keys() | corners.keys()): + pose = selected.get(role) + tag = tags[role] + points = corners.get(role) + # estimate() only runs for present corners. A missing fixed Tag may + # use a frozen pose, but its tracker's previous candidates are not + # observations of this image and must never be copied into this row. + snapshot = None if points is None else tracker.observation_snapshot(role, stamp_ns) + candidates, diagnostics, corrections = snapshot if snapshot is not None else ((), {}, 0) + source = "cached_fixed_reference" if role in cached_roles else "current_image" + if pose is not None and diagnostics.get("fixed_reference_verified") is True: + source = "verified_fixed_reference" + if diagnostics.get("frozen_image_model") is not None: + # A shared hinge model produces a current-image constrained pose; + # it must never be described as one of the raw IPPE candidates. + source = "image_constrained_current_image" + evidence[role] = { + "tag_role": role, + "tag_id": tag.tag_id, + "tag_size_m": float(tag.size_m), + "pose_source": source, + "corners_xy": None if points is None else np.asarray(points, dtype=float).tolist(), + "selected_pose": None if pose is None else _pose_payload(pose), + # The tracker retains positive-depth candidates that pass its + # reprojection gate, before independent tilt/continuity selection. + "reprojection_valid_candidates": [_pose_payload(item) for item in candidates], + "candidate_diagnostics": dict(diagnostics), + "branch_corrections_total": int(corrections), + } + if source == "image_constrained_current_image": + evidence[role].update(frozen_image_model=diagnostics["frozen_image_model"], + image_model_sha256=diagnostics.get("image_model_sha256")) + if role in cached_roles: + evidence[role]["reference_branch_revision"] = (reference_revisions or {})[role] + elif source == "verified_fixed_reference": + evidence[role]["reference_branch_revision"] = diagnostics["branch_revision"] + return evidence + + +def reference_branch_revisions(rows: Sequence[Mapping], *, profile, + require_motion_evidence=False) -> dict[tuple[str, str], int]: + """Extract one confirmed physical-Tag revision from every reference image.""" + result = {} + for row in rows: + evidence = row.get("pnp_observation_evidence", {}) + if set(evidence) != {"parent", "child"}: + raise ValueError("joint_zero_branch_evidence_missing") + joint = row["joint"] + names = (joint, profile.measurement.cross_view_sources.get(joint)) + spec = next((profile.measurement.measurements[name] for name in names + if name is not None and profile.measurement.measurements[name].view == row["view"]), None) + if spec is None: + raise ValueError("joint_zero_branch_view_invalid") + for side, role in (("parent", spec.parent_role), ("child", spec.child_role)): + item = evidence[side] + if item.get("tag_role") != role: + raise ValueError("joint_zero_branch_tag_binding_changed") + source = item["pose_source"] + if source in {"cached_fixed_reference", "verified_fixed_reference"}: + if not any(tag.role == role and tag.fixed_reference for view in profile.vision.views + if view.name == row["view"] for tag in view.tags): + raise ValueError("joint_zero_branch_cached_moving_tag") + revision = item.get("reference_branch_revision") + if source != "cached_fixed_reference": + diagnostics = item["candidate_diagnostics"] + if (diagnostics.get("observation_stamp_ns") != row["image_stamp_ns"] + or diagnostics.get("branch_status") != "tracking"): + raise ValueError("joint_zero_branch_observation_unconfirmed") + if source == "verified_fixed_reference" and ( + diagnostics.get("fixed_reference_verified") is not True + or revision != diagnostics.get("branch_revision")): + raise ValueError("joint_zero_fixed_reference_unverified") + revision = diagnostics.get("branch_revision") + key = (row["view"], item["tag_role"]) + fixed = any(tag.role == role and tag.fixed_reference for view in profile.vision.views + if view.name == row["view"] for tag in view.tags) + if require_motion_evidence and not fixed: + from ..core.domain.reference import JointZeroBranchReference + diagnostics = item.get("candidate_diagnostics", {}) + if diagnostics.get("motion_evidence_verified") is not True: + raise ValueError("joint_zero_motion_evidence_unconfirmed") + JointZeroBranchReference(*key, revision, diagnostics.get("motion_evidence_id")) + if isinstance(revision, bool) or not isinstance(revision, int) or revision < 0: + raise ValueError("joint_zero_branch_revision_invalid") + if key in result and result[key] != revision: + raise ValueError(f"joint_zero_branch_revision_changed:{key[0]}:{key[1]}") + result[key] = revision + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py index da2023f..a9c5dff 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -11,6 +11,18 @@ def reason_zh( ) -> tuple[str, str, str]: """Map a common runtime reason to a stable code, explanation and action.""" reason = str(status.get("reason", "unknown")) + if "tag_installation_not_rigid:" in reason: + return ("FIT-RIGID-CHAIN-510", "采集已完成，但实测 Tag 轨迹与整条 URDF 运动链未通过刚性一致性验证。", + "查看 tag_installation_diagnostics.json 中全部 Tag 的角度和位置残差，核对坐标转换、视觉重建和模型几何；这不等于已确认硬件松动，不应直接重复整场采集。") + if reason.startswith("joint_zero_motion_unresolved:"): + return ("OBS-JOINT-BRANCH-111", f"归零准备运动未能可靠确认当前关节的姿态分支：{reason}", + "查看所列关节、机位及 motion_branch_initialization 记录；检查 Tag 刚性、完整可见性和运动几何证据。保持当前位置，不把分数较低的候选直接当作真值，不覆盖已冻结零位。") + if reason.startswith("fixed_reference_pose_inconsistent:"): + return ("OBS-BASE-POSE-110", f"固定基准的当前图像持续无法验证已冻结姿态：{reason}", + "已保持当前位置并禁止发布。检查所列固定 Tag 的遮挡、变形、成像及相机/支架是否移动；不重新定位基准来拼接旧样本。") + if reason.startswith(("pose_branch_conflict:", "pose_branch_recovery_required:")): + return ("OBS-POSE-BRANCH-109", f"当前所需 Tag 的姿态分支与已冻结参考无法可靠保持一致：{reason}", + "已保持当前位置并禁止发布。检查所列机位和 Tag 的刚性、视角与成像；不重置既有零位，不用镜像姿态继续拼接数据。") if "joint_zero_unavailable" in reason: return ("OBS-JOINT-ZERO-107", f"当前关节未取得稳定零位观测：{reason}", "查看所列关节、机位和 Tag；排除持续遮挡或抖动后重新开始，已有零位不会被覆盖。") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py index 4c63766..a300be8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py @@ -11,7 +11,9 @@ from typing import Any, Mapping, Sequence import numpy as np -from .engine import ACQUISITION_POLICY_VERSION +from ..core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from .engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION +from .diagnostic_capture import reject_diagnostic_capture def discover_resume_candidate( @@ -59,6 +61,7 @@ def discover_resume_candidate( row = json.loads(line) if not isinstance(row, dict): raise ValueError("invalid checkpoint record") + reject_diagnostic_capture((row,)) kind = str(row.get("kind", "")) if kind == "session_start": if start is not None: @@ -92,6 +95,10 @@ def discover_resume_candidate( continue if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: continue + if start.get("capture_schedule_version") not in {CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION}: + continue + if start.get("pose_tracking_policy_version") != POSE_TRACKING_POLICY_VERSION: + continue recorded = start.get("protected_hashes", start) if not isinstance(recorded, Mapping) or any(recorded.get(key) != value for key, value in protected_hashes.items()): continue diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py index eab7d6f..595b2cc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py @@ -2,9 +2,7 @@ import time -from apriltag_msgs.msg import AprilTagDetectionArray -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import CameraInfo, JointState @@ -13,8 +11,10 @@ from std_srvs.srv import Trigger from ..adapters.ros_binding import SdkBindingPorts, bind_ros_sdk from ..coordinator import CalibrationCoordinator +from ..branch_worker import IsolatedBranchSolver +from ...core.geometry.tag_pose.image_projection_worker import ImageProjectionWorker from ..inputs import RuntimePorts -from .io import RosCalibrationIO, _stamp_ns, camera_input, detection_input +from .io import RosCalibrationIO, _stamp_ns, camera_input, checked_detection_input from .parameters import load_runtime_parameters, parameter_defaults @@ -24,9 +24,6 @@ class UnifiedCalibrationNode(Node): for name, default in parameter_defaults(profile).items(): self.declare_parameter(name, default) parameters = load_runtime_parameters(profile, lambda name: self.get_parameter(name).value) - self.motion_callback_group = MutuallyExclusiveCallbackGroup() - self.vision_callback_groups = { - view: MutuallyExclusiveCallbackGroup() for view in profile.vision.view_names} self.io = RosCalibrationIO(self, profile, parameters) ports = RuntimePorts(lambda: int(self.get_clock().now().nanoseconds), time.monotonic, lambda: self.count_publishers(parameters.command_topic), @@ -35,57 +32,88 @@ class UnifiedCalibrationNode(Node): def subscribe_health(receive): self.create_subscription(String, parameters.command_topic.rsplit("/", 1)[0]+"/calibration_health", - lambda message: receive(message.data), 10, callback_group=self.motion_callback_group) + lambda message: receive(message.data), 10) def adapter_factory(publish, set_speed, feedback_fresh): return bind_ros_sdk(profile, SdkBindingPorts(feedback_fresh, ports.monotonic, subscribe_health), publish=publish, set_speed=set_speed) - self.coordinator = CalibrationCoordinator(profile, parameters, ports, adapter_factory) - self.create_subscription(JointState, parameters.state_topic, self._state_callback, 30, - callback_group=self.motion_callback_group) - for view in profile.vision.view_names: + self.branch_solver = IsolatedBranchSolver(parameters.session_dir / "motion_solver_diagnostic.jsonl") + self.projection_worker = ImageProjectionWorker() + from ..artifacts.controller import FinalizationController + self.coordinator = CalibrationCoordinator(profile, parameters, ports, adapter_factory, + finalization=FinalizationController(profile, parameters.session_dir, isolated=True), + initialization_solver=self.branch_solver, projection_worker=self.projection_worker) + from ..detection_dispatch import DetectionDispatcher + self.detection_dispatch = DetectionDispatcher(self.coordinator.required_observation_views, + self.coordinator.receive_detections, + maximum_wait_seconds=parameters.maximum_state_image_skew_ns/1e9, + failed=lambda error: self.get_logger().error(f"image processing failed: {error}")) + self.fixed_ids = {view.name: frozenset(t.tag_id for t in view.tags if t.fixed_reference) + for view in profile.vision.views} + if profile.sdk_adapter == "legacy_byte_sdk": + self.create_subscription(String, parameters.state_topic+"_timed", self._timed_state_callback, 30) + else: + self.create_subscription(JointState, parameters.state_topic, self._state_callback, 30) + for view in self.coordinator.required_observation_views: self.create_subscription(CameraInfo, parameters.camera_info_topics[view], lambda message, selected=view: self._camera_info_callback(selected, message), - qos_profile_sensor_data, callback_group=self.vision_callback_groups[view]) - self.create_subscription(AprilTagDetectionArray, parameters.detection_topics[view], + qos_profile_sensor_data) + self.create_subscription(String, parameters.detection_topics[view]+"_checked", lambda message, selected=view: self._detections_callback(selected, message), - qos_profile_sensor_data, callback_group=self.vision_callback_groups[view]) - self.create_service(Trigger, f"{profile.namespace}/start", self._start, - callback_group=self.motion_callback_group) - self.create_service(Trigger, f"{profile.namespace}/abort", self._abort, - callback_group=self.motion_callback_group) - self.create_timer(1.0/parameters.command_rate_hz, self._tick, - callback_group=self.motion_callback_group) + qos_profile_sensor_data) + self.create_service(Trigger, f"{profile.namespace}/start", self._start) + self.create_service(Trigger, f"{profile.namespace}/abort", self._abort) + self.create_timer(1.0/parameters.command_rate_hz, self._tick) self.create_timer(0.5, self._publish_status) def _start(self, _request, response): + self.detection_dispatch.clear() result = self.coordinator.start() response.success, response.message = result.success, result.message return response def _abort(self, _request, response): + self.detection_dispatch.clear() result = self.coordinator.abort() response.success, response.message = result.success, result.message return response def _state_callback(self, message): - self.coordinator.receive_feedback(tuple(message.name), tuple(message.position), - _stamp_ns(message.header.stamp)) + self._receive_feedback(tuple(message.name), tuple(message.position), _stamp_ns(message.header.stamp)) + + def _timed_state_callback(self, message): + from ..feedback import decode_timed_feedback + try: + names, positions, stamp, channel_stamps = decode_timed_feedback(message.data) + except (ValueError, TypeError) as error: + self.get_logger().error(str(error)) + return # Invalid data cannot refresh the independent feedback watchdog. + self._receive_feedback(names, positions, stamp, channel_stamps_ns=channel_stamps) + + def _receive_feedback(self, names, positions, stamp, *, channel_stamps_ns=()): + sample = self.coordinator.receive_feedback(names, positions, stamp, channel_stamps_ns=channel_stamps_ns) + if sample is not None: + self.detection_dispatch.feedback_available(min(sample.channel_stamps_ns or (sample.stamp_ns,))) def _camera_info_callback(self, view, message): self.coordinator.receive_camera_info(view, camera_input(message)) def _detections_callback(self, view, message): - self.coordinator.receive_detections(detection_input(view, message)) + self.detection_dispatch.submit(checked_detection_input(view, message, + fixed_ids=self.fixed_ids[view])) def _tick(self): + self.detection_dispatch.raise_if_failed() self.coordinator.tick() def _publish_status(self): self.io.publish_status(self.coordinator.snapshot()) def destroy_node(self): + self.detection_dispatch.close() + self.branch_solver.close() + self.projection_worker.close() self.coordinator.close() return super().destroy_node() @@ -94,7 +122,10 @@ def run_profile_node(profile, args=None): import rclpy rclpy.init(args=args) node = UnifiedCalibrationNode(profile) - executor = MultiThreadedExecutor(num_threads=4) + # ROS callbacks only decode messages and advance the control state. Heavy + # image work has its own bounded workers. Parallel ROS callbacks would + # compete for the same session lock and delay feedback and command ticks. + executor = SingleThreadedExecutor() executor.add_node(node) try: executor.spin() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py index edba2c1..dfa52a1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py @@ -20,10 +20,29 @@ def camera_input(message) -> CameraModelInput: tuple(message.k), tuple(message.d), tuple(message.r), tuple(message.p)) -def detection_input(view: str, message) -> DetectionInput: +def detection_input(view: str, message, *, gray=None, fixed_ids=()) -> DetectionInput: + from ...core.geometry.tag_border import inspect_tag_border + def convert(tag): + corners = tuple((float(p.x), float(p.y)) for p in tag.corners) + border = None + if int(tag.id) in fixed_ids: + border = ({"passed": False, "reason": "border_image_missing"} if gray is None + else inspect_tag_border(gray, corners)) + return TagDetection(int(tag.id), corners, int(tag.hamming), float(tag.decision_margin), border) return DetectionInput(view, _stamp_ns(message.header.stamp), tuple( - TagDetection(int(tag.id), tuple((float(p.x), float(p.y)) for p in tag.corners), - int(tag.hamming), float(tag.decision_margin)) for tag in message.detections)) + convert(tag) for tag in message.detections)) + + +def checked_detection_input(view, message, *, fixed_ids): + data = json.loads(message.data) + if data.get("policy") != "image_border_checked_v1" or data.get("view") != view: + raise ValueError("checked detection provenance differs from subscribed view") + detections = tuple(TagDetection(int(tag["tag_id"]), tuple(map(tuple, tag["corners"])), + int(tag["hamming"]), float(tag["decision_margin"]), tag.get("border_quality")) + for tag in data["detections"]) + if any(tag.tag_id in fixed_ids and not isinstance(tag.border_quality, dict) for tag in detections): + raise ValueError("fixed Tag is missing its current-image border check") + return DetectionInput(view, int(data["stamp_ns"]), detections) class RosCalibrationIO: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py index 7ec295e..c9ee945 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py @@ -32,6 +32,7 @@ def parameter_defaults(profile) -> dict[str, Any]: "state_topic": f"/{model}/cb_right_hand_state", "setting_topic": f"/{model}/cb_hand_setting_cmd", "commands_enabled": True, + "diagnostic_capture": "", "speed_settle_seconds": float( speed_parameters.get("speed_settle_seconds", 0.2) ), @@ -47,6 +48,9 @@ def parameter_defaults(profile) -> dict[str, Any]: "pnp_maximum_translation_jump_m": DEFAULT_POSE_TRACKING_PARAMETERS.maximum_translation_jump_m, "pnp_maximum_tag_tilt_deg": math.degrees(DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad), "pnp_tracker_reset_seconds": DEFAULT_POSE_TRACKING_PARAMETERS.reset_after_seconds, + "pnp_branch_confirmation_frames": DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_frames, + "pnp_branch_confirmation_minimum_seconds": DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_minimum_seconds, + "pnp_branch_equivalence_deg": math.degrees(DEFAULT_POSE_TRACKING_PARAMETERS.branch_equivalence_rad), "command_rate_hz": float(speed_parameters.get("command_rate_hz", 100.0)), } for view in profile.vision.view_names: @@ -60,6 +64,12 @@ def parameter_defaults(profile) -> dict[str, Any]: def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimeParameters: + from ..diagnostic_capture import resolve_diagnostic_capture + + diagnostic_capture = str(value("diagnostic_capture")).strip() + diagnostic = resolve_diagnostic_capture(profile, diagnostic_capture) + if diagnostic is not None and not bool(value("commands_enabled")): + raise ValueError("diagnostic_capture requires commands_enabled") serial_number = str(value("serial_number")) session_dir = Path(str(value("session_dir"))).expanduser().resolve() source_urdf = Path(str(value("source_urdf_path"))).expanduser().resolve() @@ -102,6 +112,8 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara value("profile_config_expected_sha256") ) resume_value = str(value("resume_raw_samples_path")).strip() + if diagnostic is not None and resume_value: + raise ValueError("diagnostic_capture does not support resuming prior samples") resume_raw_samples_path = ( None if not resume_value @@ -145,6 +157,7 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara detection=DetectionPolicy(int(value("maximum_hamming")), float(value("minimum_decision_margin")), float(value("minimum_edge_pixels"))), tracking=tracking_parameters(value), + diagnostic_capture=diagnostic_capture, ) @@ -157,4 +170,7 @@ def tracking_parameters(value: Callable[[str], Any]) -> PoseTrackingParameters: maximum_translation_jump_m=float(value("pnp_maximum_translation_jump_m")), maximum_tag_tilt_rad=math.radians(float(value("pnp_maximum_tag_tilt_deg"))), reset_after_seconds=float(value("pnp_tracker_reset_seconds")), + branch_confirmation_frames=int(value("pnp_branch_confirmation_frames")), + branch_confirmation_minimum_seconds=float(value("pnp_branch_confirmation_minimum_seconds")), + branch_equivalence_rad=math.radians(float(value("pnp_branch_equivalence_deg"))), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/tag_border_filter.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/tag_border_filter.py new file mode 100644 index 0000000..fa715f5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/tag_border_filter.py @@ -0,0 +1,71 @@ +"""Image preprocessing in its own process, isolated from SDK/control callbacks.""" + +import argparse +from dataclasses import asdict +import hashlib +import json +from pathlib import Path + +from apriltag_msgs.msg import AprilTagDetectionArray +from cv_bridge import CvBridge +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image +from std_msgs.msg import String + +from ...profiles import load_hand_profile +from ..image_pairing import ExactImagePairing +from .io import detection_input + + +class TagBorderFilter(Node): + def __init__(self, profile, views): + super().__init__("tag_border_filter") + self.bridge = CvBridge() + self.pairing = {view: ExactImagePairing() for view in views} + self.fixed_ids = {v.name: frozenset(t.tag_id for t in v.tags if t.fixed_reference) + for v in profile.vision.views} + self.publishers_by_view = {} + for view in views: + raw = f"{profile.namespace}/{view}/apriltag/detections" + self.publishers_by_view[view] = self.create_publisher(String, raw+"_checked", 10) + self.create_subscription(Image, f"{profile.namespace}/{view}/camera/image_rect", + lambda msg, v=view: self.receive(v, "image", msg), qos_profile_sensor_data) + self.create_subscription(AprilTagDetectionArray, raw, + lambda msg, v=view: self.receive(v, "detections", msg), qos_profile_sensor_data) + + def receive(self, view, kind, message): + stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec + pair = self.pairing[view].add(kind, stamp, message) + if pair is None: + return + image, detections = pair + gray = self.bridge.imgmsg_to_cv2(image, desired_encoding="mono8") + observation = detection_input(view, detections, gray=gray, fixed_ids=self.fixed_ids[view]) + output = String() + output.data = json.dumps({"policy": "image_border_checked_v1", **asdict(observation)}, + separators=(",", ":"), allow_nan=False) + self.publishers_by_view[view].publish(output) + + +def main(args=None): + parser = argparse.ArgumentParser() + parser.add_argument("--profile-config", required=True) + parser.add_argument("--profile-sha256", required=True) + parser.add_argument("--views", nargs="+", required=True) + selected, ros_args = parser.parse_known_args(args) + path = Path(selected.profile_config) + if hashlib.sha256(path.read_bytes()).hexdigest() != selected.profile_sha256: + raise ValueError("border filter Profile hash differs from launch") + profile = load_hand_profile(path) + if not set(selected.views) <= set(profile.vision.view_names): + raise ValueError("border filter view is not declared by Profile") + rclpy.init(args=ros_args) + node = TagBorderFilter(profile, selected.views) + try: + rclpy.spin(node) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 14799b8..6d73682 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -7,6 +7,7 @@ hardware. Pending legacy status/artifact contracts are isolated in compat. from __future__ import annotations import argparse +import json import os from pathlib import Path import subprocess @@ -19,7 +20,7 @@ from ..storage import atomic_write_json from .status import normalize_status, render_status_zh -TERMINAL_STATES = frozenset({"COMPLETE", "PAUSED", "ABORTED", "FAILED"}) +TERMINAL_STATES = frozenset({"COMPLETE", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"}) def drive_session( @@ -102,6 +103,7 @@ def _write_failure(config: ProductConfig, session: Path, status: Mapping[str, An def run_online( config: ProductConfig, *, record_bag: bool = False, commands_enabled: bool = True, allow_resume: bool = True, + diagnostic_capture: str = "", ) -> int: import rclpy from std_srvs.srv import Trigger @@ -109,6 +111,7 @@ def run_online( from .artifacts.completion import finish_online_artifacts from ..hikrobot_camera import configure_fastdds_large_image_transport from .resume import discover_resume_candidate + from .diagnostic_capture import diagnostic_capture_label, resolve_diagnostic_capture from .adapters.ros_topics import sdk_topics from .runner_support import ( CalibrationMonitor, ProgressConsole, create_session_directory, @@ -116,24 +119,26 @@ def run_online( ) profile = config.calibration_contract.typed_profile + diagnostic = resolve_diagnostic_capture(profile, diagnostic_capture) + if diagnostic is not None and not commands_enabled: + raise ValueError("diagnostic_capture requires commands_enabled") + session = create_session_directory(config.session_root) + from ..profiles.validator import validate_executable_profile, require_release_geometry + contract_report = validate_executable_profile(profile, config.source_urdf) + atomic_write_json(session/"measurement_contract.json", contract_report) + if profile.artifacts.output_schema_version >= 3 and commands_enabled and diagnostic is None: + require_release_geometry(contract_report) resume = discover_resume_candidate( config.session_root, profile_id=profile.key.profile_id, serial_number=config.serial_number, protected_hashes=protected_inputs(config), - ) if allow_resume and commands_enabled else None - session = create_session_directory(config.session_root) - from ..profiles.validator import validate_executable_profile - contract_report = validate_executable_profile(profile, config.source_urdf) - atomic_write_json(session/"measurement_contract.json", contract_report) - coverage = contract_report["independent_measurement_coverage"] - if profile.artifacts.output_schema_version >= 3 and (coverage["unresolved_motion_joints"] or coverage["missing_baseline_geometry"]): - print("当前 Profile 仍有未实测且未声明迁移的关节，或缺少绝对零位来源；本次可采集诊断数据，但不能发布完整 v3 JSON/URDF；" - "缺失关节见 measurement_contract.json。", flush=True) + ) if allow_resume and commands_enabled and diagnostic is None else None if contract_report["cad_constraint_conflicts"]: print("原始 CAD 存在限位/mimic 冲突，未修改原文件；详见 measurement_contract.json。最终范围验证不通过时禁止发布。", flush=True) log_path = session / "calibration.log" configure_fastdds_large_image_transport() process = None monitor = None + capture_complete = False owns_context = not rclpy.ok() with log_path.open("a", encoding="utf-8", buffering=1) as log_stream: try: @@ -164,9 +169,19 @@ def run_online( print(f"发现候选断点：{resume.name}；尚未复用，启动后验证基准和 Tag 安装关系。", flush=True) if not commands_enabled: print("仅预览：不自动开始、不发送标定运动；Ctrl+C 退出。", flush=True) + if diagnostic is not None: + if diagnostic.hold_command_values: + values = "/".join(f"{value:g}" for value in diagnostic.hold_command_values) + print(f"{diagnostic_capture_label(diagnostic.mode)}：仅执行 yaw 一次往返；" + f"每个方向扫描后回到该方向起点，按方向依次到达 {values}，各保持 {diagnostic.hold_seconds:g} 秒；" + "不复用断点、不拟合或发布标定产物。", flush=True) + else: + print("拇指短程诊断：仅执行拇指任务各一次往返，保留准备和零位采集；不复用断点、不拟合或发布标定产物。", flush=True) + launch_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} process = subprocess.Popen( launch_command(config, session, record_bag=record_bag, - commands_enabled=commands_enabled, resume_from=resume), + commands_enabled=commands_enabled, resume_from=resume, + **launch_options), cwd=config.workspace, env=environment, stdout=log_stream, stderr=subprocess.STDOUT, text=True, start_new_session=True, ) @@ -176,6 +191,35 @@ def run_online( auto_start=commands_enabled, ) atomic_write_json(session / "node_status.json", status) + if diagnostic is not None: + if normalize_status(status)["state"] != "DIAGNOSTIC_COMPLETE": + _write_failure(config, session, status) + return 3 + report_path = session / "diagnostic_capture.json" + report = json.loads(report_path.read_text(encoding="utf-8")) + if (not isinstance(report, dict) + or report.get("mode") != diagnostic_capture + or report.get("result") != "DIAGNOSTIC_COMPLETE" + or report.get("publication_allowed") is not False): + raise ValueError("diagnostic_capture_completion_report_invalid") + capture_complete = True + capture_label = diagnostic_capture_label(diagnostic.mode) if diagnostic.hold_command_values else "拇指诊断" + print(f"{capture_label}数据已采完；本次未进行精度验收，未发布 JSON 或修正 URDF。\n诊断报告：{report_path}\n原始样本：{session / 'raw_samples.jsonl'}", flush=True) + pose_gaps = report.get("accepted_pose_quality_incomplete_attempts", 0) + unresolved = report.get("missing_joint_zero_references", ()) + if unresolved: + print("零位未确认，仅保留原始观测：" + ", ".join(unresolved) + + "；未生成这些关节的零位或角度样本。", flush=True) + if pose_gaps: + print(f"其中 {pose_gaps} 次方向采集的可信姿态覆盖不足；已保留原始候选供诊断，不能据此确认标定精度。", flush=True) + # The expensive diagnostic is independent of live sampling. + # Stop camera/SDK processes first; it can never delay feedback + # or change a reference after the capture has completed. + stop_stack(process) + process = None + from .diagnostic_analysis import finish_diagnostic_analysis + finish_diagnostic_analysis(profile, session) + return 0 if normalize_status(status)["state"] != "COMPLETE": _write_failure(config, session, status) return 3 @@ -187,10 +231,11 @@ def run_online( print(f"{name}：{path}", flush=True) return 0 except KeyboardInterrupt: - if monitor is not None and monitor.abort_client.service_is_ready(): + if not capture_complete and process is not None and monitor is not None and monitor.abort_client.service_is_ready(): monitor.abort_client.call_async(Trigger.Request()) rclpy.spin_once(monitor, timeout_sec=0.2) - print(f"已请求中止；本次数据保留在 {session}。", flush=True) + message = "诊断采集已完成；已取消后续分析" if capture_complete else "已请求中止" + print(f"{message}；本次数据保留在 {session}。", flush=True) return 130 except Exception as error: import traceback @@ -219,19 +264,39 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--record-bag", action="store_true") parser.add_argument("--commands-disabled", action="store_true") parser.add_argument("--no-resume", action="store_true") + diagnostic_options = parser.add_mutually_exclusive_group() + diagnostic_options.add_argument("--diagnostic-thumb", action="store_true", + help="仅 O6 右手：拇指任务各采一次往返，只保存诊断数据") + diagnostic_options.add_argument("--diagnostic-yaw-hold", action="store_true", + help="仅 O6 右手：yaw 两方向扫描后各在 64/128/192 保持 10 秒，只保存诊断数据") parser.add_argument("--offline-raw", default="") parser.add_argument("--offline-output", default="") parser.add_argument("--publish-offline", action="store_true") selected = parser.parse_args(args) + diagnostic_flag = ("--diagnostic-thumb" if selected.diagnostic_thumb else + "--diagnostic-yaw-hold" if selected.diagnostic_yaw_hold else "") + if diagnostic_flag and (selected.offline_raw or selected.offline_output + or selected.publish_offline or selected.commands_disabled): + parser.error(f"{diagnostic_flag} cannot be combined with --offline-raw, --offline-output, --publish-offline or --commands-disabled") config = load_product_config( selected.config or str(default_product_config_path()), workspace=selected.workspace, check_can=not (selected.validate_only or selected.offline_raw or selected.commands_disabled), ) + diagnostic_capture = ("o6_thumb" if selected.diagnostic_thumb else + "o6_yaw_hold" if selected.diagnostic_yaw_hold else "") + diagnostic = None + if diagnostic_capture: + from .diagnostic_capture import resolve_diagnostic_capture + try: + diagnostic = resolve_diagnostic_capture(config.calibration_contract.typed_profile, diagnostic_capture) + except ValueError as error: + parser.error(str(error)) if selected.validate_only: - import json from ..profiles.validator import validate_executable_profile - print(json.dumps(validate_executable_profile(config.calibration_contract.typed_profile, - config.source_urdf), ensure_ascii=False, indent=2)) + report = validate_executable_profile(config.calibration_contract.typed_profile, config.source_urdf) + if diagnostic is not None: + report["diagnostic_capture"] = {**diagnostic.as_dict(), "publication_allowed": False} + print(json.dumps(report, ensure_ascii=False, indent=2)) print(f"配置与受保护输入一致：{config.profile_key.profile_id}。此检查不代表 URDF 精度或硬件验收通过。") return if selected.offline_raw: @@ -242,6 +307,8 @@ def main(args: list[str] | None = None) -> None: return if selected.offline_output or selected.publish_offline: parser.error("--offline-output/--publish-offline require --offline-raw") + online_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} raise SystemExit(run_online(config, record_bag=selected.record_bag, commands_enabled=not selected.commands_disabled, - allow_resume=not selected.no_resume)) + allow_resume=not selected.no_resume and diagnostic is None, + **online_options)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py index 05b0b21..801674f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py @@ -95,6 +95,7 @@ def launch_command( commands_enabled: bool, sdk_startup_speed_u8: int | None = None, resume_from: Path | None = None, + diagnostic_capture: str = "", ) -> list[str]: """Build the single launch invocation entirely from the product contract.""" arguments = { @@ -122,13 +123,14 @@ def launch_command( "vendor_sdk_python_package": str(getattr(config, "sdk_python_package", None) or ""), "sdk_package_expected_sha256": getattr(config, "sdk_package_sha256", ""), "record_bag": str(record_bag).lower(), + "diagnostic_capture": diagnostic_capture, } startup_speed = sdk_startup_speed_u8 if startup_speed is None: startup_speed = config.calibration_contract.typed_profile.motion.speed_parameters.get("baseline_u8") if startup_speed is not None: arguments["calibration_speed"] = str(int(startup_speed)) - if resume_from is not None: + if resume_from is not None and not diagnostic_capture: arguments["resume_raw_samples_path"] = str( resume_from / "raw_samples.jsonl" ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py index 652893f..41d354b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py @@ -8,17 +8,13 @@ from typing import Mapping, Sequence from .engine import CalibrationEngine, ScanUnit, SweepQuality -def observation_streams(profile, task): - """(record field, joint identity, measurement) required by one task.""" - for name in task.joints: - yield "joint", name, profile.measurement.measurements[name] - secondary = profile.measurement.cross_view_sources.get(name) - if secondary is not None: - yield "observation_joint", name, profile.measurement.measurements[secondary] +from ..core.domain.profile import observation_streams as observation_streams def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, - records: Sequence[Mapping], *, first_cycle_spans: dict[str, float]) -> SweepQuality: + records: Sequence[Mapping], *, first_cycle_spans: dict[str, float], + include_steady: bool = True, +) -> SweepQuality: """No rates, coupling, endpoint tracking error or Tag percentages stop a scan. Count unique images separately for EVERY required joint/view. A good @@ -33,6 +29,21 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, if span <= 0: raise ValueError("a scan needs a nonzero declared range") failures, warnings, metrics, references = [], [], {}, {} + if unit.sample_phase == "steady": + from .steady import steady_failures, steady_targets + failures = steady_failures(profile, unit, attempt, records) + targets = steady_targets(profile, unit, records=records) + for field, name, spec in observation_streams(profile, task): + rows = [r for r in records if r.get(field) == name and r.get("view") == spec.view + and r.get("sample_phase") == "steady" and r.get("task_name") == unit.task_key + and r.get("cycle") == unit.cycle and r.get("direction") == unit.direction + and int(r.get("attempt", 1)) == attempt] + covered = {r["steady_index"] for r in rows} + metrics[f"{field}:{name}:{spec.view}:{unit.direction}"] = { + "quality_scope": "steady_command_nodes", "valid_frames": len({r["image_stamp_ns"] for r in rows}), + "feedback_span": len(covered) / len(targets), "feedback_bins": len(covered), + "maximum_bin_gap": 0, "required_nodes": len(targets)} + return SweepQuality(not failures, tuple(failures), (), metrics) for field, name, spec in observation_streams(profile, task): identity = f"{field}:{name}:{spec.view}:{unit.direction}" samples = {} @@ -68,8 +79,9 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, warnings.extend(f"{identity}:{reason}" for reason in decision.warnings) metrics[identity] = dict(decision.metrics) references[identity] = float(decision.metrics["feedback_span"]) - from .steady import steady_failures - failures.extend(steady_failures(profile, unit, attempt, records)) + if include_steady and profile.acquisition.command_capture_mode == "interleaved": + from .steady import steady_failures + failures.extend(steady_failures(profile, unit, attempt, records)) if not failures and unit.cycle == 0: first_cycle_spans.update(references) return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py index c643c29..bdfeecf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -11,7 +11,9 @@ from enum import Enum from typing import Mapping from ..core import CalibrationProfile, CalibrationStatus, PauseStatus, TaskStatus +from .diagnostic_capture import DiagnosticCapturePlan from .engine import CalibrationEngine, ScanUnit, SweepQuality +from .observation_scope import required_observation_views from .resume import ResumeDecision @@ -34,6 +36,7 @@ class CalibrationPhase(str, Enum): VALIDATE_URDF = "VALIDATE_URDF" PUBLISH = "PUBLISH" COMPLETE = "COMPLETE" + DIAGNOSTIC_COMPLETE = "DIAGNOSTIC_COMPLETE" PAUSED = "PAUSED" ABORTED = "ABORTED" FAILED = "FAILED" @@ -49,9 +52,14 @@ class SessionAction: class CalibrationSession: """Deterministic orchestration kernel with no model-specific branches.""" - def __init__(self, profile: CalibrationProfile) -> None: + def __init__( + self, profile: CalibrationProfile, *, + diagnostic_capture: DiagnosticCapturePlan | None = None, + ) -> None: self.profile = profile - self.engine = CalibrationEngine(profile) + self.diagnostic_capture = diagnostic_capture + self.required_observation_views = required_observation_views(profile, diagnostic_capture) + self.engine = CalibrationEngine(profile, diagnostic_capture=diagnostic_capture) self.phase = CalibrationPhase.WAIT_DEVICE self._units = self.engine.scan_units() self._unit_index = 0 @@ -81,6 +89,8 @@ class CalibrationSession: def start(self, *, resume_requested: bool = False) -> None: self._require(CalibrationPhase.READY) + if self.diagnostic_capture is not None and resume_requested: + raise ValueError("diagnostic capture cannot resume previous data") self._resume_requested = bool(resume_requested) self._reference_locked = False self.phase = CalibrationPhase.BASELINE @@ -109,6 +119,7 @@ class CalibrationSession: ) def resume_checked(self, decision: ResumeDecision) -> None: + self._require_formal_resume() self._require(CalibrationPhase.RESUME_VERIFY) if self.profile.artifacts.output_schema_version >= 3 and decision.completed_units: raise ValueError("v3 resume requires per-joint zero verification before skipping units") @@ -150,6 +161,7 @@ class CalibrationSession: def authorize_task_reuse(self, units): """Only the coordinator can call this after checking frozen joint zeros.""" + self._require_formal_resume() self._require_any(CalibrationPhase.PREPARE, CalibrationPhase.MAPPING_PROBE) current = self.current_unit valid = {(u.task_key, u.cycle, u.direction) for u in self._units} @@ -205,7 +217,11 @@ class CalibrationSession: def return_complete(self) -> None: self._require(CalibrationPhase.RETURN_BASELINE) - self.phase = CalibrationPhase.FIT + self.phase = ( + CalibrationPhase.DIAGNOSTIC_COMPLETE + if self.diagnostic_capture is not None + else CalibrationPhase.FIT + ) def fit_complete(self) -> None: self._require(CalibrationPhase.FIT) @@ -244,8 +260,9 @@ class CalibrationSession: self._pause = PauseStatus("operator_abort", "操作者已中止标定") self.phase = CalibrationPhase.ABORTED - def fail(self, code: str, reason: str) -> None: - self._pause = PauseStatus(code, reason) + def fail(self, code: str, reason: str, details: Mapping[str, object] | None = None, + *, suggestion: str = "") -> None: + self._pause = PauseStatus(code, reason, suggestion, details or {}) self.phase = CalibrationPhase.FAILED def action(self) -> SessionAction: @@ -274,13 +291,13 @@ class CalibrationSession: ) progress = ( 1.0 - if self.phase == CalibrationPhase.COMPLETE + if self.phase in {CalibrationPhase.COMPLETE, CalibrationPhase.DIAGNOSTIC_COMPLETE} else 0.90*self._unit_index / max(1, len(self._units)) ) required: dict[str, tuple[int, ...]] = {} if self.phase in {CalibrationPhase.REFERENCE_LOCKING, CalibrationPhase.RESUME_VERIFY}: required = {view.name: tuple(tag.tag_id for tag in view.tags if tag.fixed_reference) - for view in self.profile.vision.views} + for view in self.profile.vision.views if view.name in self.required_observation_views} if task is not None: measurement_names = set(task.joints) measurement_names.update( @@ -313,8 +330,16 @@ class CalibrationSession: task=TaskStatus( key="" if task is None else task.key, label="" if task is None else " / ".join(task.joints), - cycle=None if unit is None else unit.cycle + 1, + cycle=None if unit is None or unit.sample_phase == "steady" else unit.cycle + 1, + cycle_count=( + len(self.diagnostic_capture.cycles) + if self.diagnostic_capture is not None + else 4 + ), direction="" if unit is None else unit.direction, + capture_stage=("command_validation" if unit is not None and unit.sample_phase == "steady" + and unit.cycle == 5 else "command_training" if unit is not None + and unit.sample_phase == "steady" else "motion"), required_tag_ids_by_view=required, ), resume={"message": self._resume_message}, @@ -328,6 +353,10 @@ class CalibrationSession: f"expected {expected.value}" ) + def _require_formal_resume(self) -> None: + if self.diagnostic_capture is not None: + raise ValueError("diagnostic capture cannot reuse previous data") + def _require_any(self, *expected: CalibrationPhase) -> None: if self.phase not in expected: allowed = ",".join(value.value for value in expected) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py index 26c00f0..d22e74f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py @@ -7,6 +7,7 @@ from typing import TYPE_CHECKING, Mapping from ..core.domain.status import CalibrationStatus, MotionStatus from .camera_preflight import CameraPreflight +from .diagnostic_quality import evaluate_diagnostic_unit from .scan_quality import evaluate_capture_unit from .session import CalibrationPhase as Phase @@ -17,6 +18,7 @@ if TYPE_CHECKING: _LEGACY_STATES = { Phase.WAIT_DEVICE: "WAIT_DEVICES", Phase.READY: "READY", Phase.COMPLETE: "PASSED", Phase.PAUSED: "PAUSED", + Phase.DIAGNOSTIC_COMPLETE: "DIAGNOSTIC_COMPLETE", Phase.ABORTED: "ABORTED", Phase.FAILED: "FAILED", **{phase: "FINALIZING" for phase in ( Phase.FIT, Phase.HOLDOUT_VALIDATE, Phase.BUILD_ARTIFACTS, @@ -57,6 +59,7 @@ class RuntimeSnapshot: steady: bool = False steady_node: int | None = None camera_preflight: CameraPreflight | None = None + diagnostic_capture: Mapping | None = None def as_dict(self) -> dict: value = self.calibration.as_dict() @@ -67,6 +70,8 @@ class RuntimeSnapshot: value["devices"] = asdict(self.devices) if self.camera_preflight is not None: value["camera_preflight"] = self.camera_preflight.as_dict() + if self.diagnostic_capture is not None: + value["diagnostic_capture"] = dict(self.diagnostic_capture) if self.steady: value["acquisition"]["steady_node"] = self.steady_node return value @@ -100,18 +105,28 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot: missing = {tag for view, ids in required.items() for tag in host.capture.last_missing.get(view, ids)} & required_ids filtered = {tag for ids in host.capture.last_filtered.values() for tag in ids} & required_ids - steady = motion is not None and motion.phase in {"steady", "steady_prepare"} + interrupted = session.phase in {Phase.PAUSED, Phase.ABORTED, Phase.FAILED} + steady = not interrupted and motion is not None and motion.phase in {"steady", "steady_prepare"} steady_node = None if steady and motion.steady_index is not None: steady_node = motion.steady_index+1 task = replace(task, label=task.label+f"（稳态点 {steady_node}）") index = None if motion is None else motion.command_index health = host.sdk_adapter.health() - quality = host.execution.last_quality if session.phase == Phase.PAUSED else None - if session.phase in {Phase.SWEEP, Phase.RESCAN, Phase.EVALUATE}: - quality = evaluate_capture_unit(profile, session.current_unit, - host.execution.action.retry+1, tuple(host._unit_rows), - first_cycle_spans=dict(host.execution.first_cycle_spans)) + # A pause can interrupt a new task before its first evaluation. The last + # completed quality result may belong to another task/direction; scope + # paused statistics to the current unit just like live scan statistics. + quality = None + diagnostic_quality = None + if session.current_unit is not None and session.phase in {Phase.SWEEP, Phase.RESCAN, Phase.EVALUATE, Phase.PAUSED}: + if host.diagnostic_capture is not None: + diagnostic_quality = evaluate_diagnostic_unit(profile, session.current_unit, + host.execution.action.retry+1, tuple(host._unit_rows)) + quality = diagnostic_quality.observations + else: + quality = evaluate_capture_unit(profile, session.current_unit, + host.execution.action.retry+1, tuple(host._unit_rows), + first_cycle_spans=dict(host.execution.first_cycle_spans), include_steady=True) acquisition = replace(status.acquisition, retry_count=host.execution.action.retry) if quality is not None and quality.metrics: metrics = tuple(quality.metrics.values()) @@ -121,7 +136,7 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot: bins=min(m["feedback_bins"] for m in metrics), maximum_gap=max(m["maximum_bin_gap"] for m in metrics)) status = replace(status, task=task, acquisition=acquisition, - phase=motion.phase if motion is not None and motion.phase in + phase=motion.phase if not interrupted and motion is not None and motion.phase in {"steady", "steady_prepare", "joint_zero", "zero_approach"} else status.phase, recognized_tag_ids=tuple(sorted(required_ids-missing-filtered)), missing_tag_ids=tuple(sorted(missing)), rejected_tag_ids=tuple(sorted(filtered)), @@ -136,11 +151,26 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot: "source_session": "" if parameters.resume_raw_samples_path is None else parameters.resume_raw_samples_path.parent.name}, outputs={"json": host.final_json, "urdf": host.final_urdf, + "raw_samples": str(host.raw_path), + "failure_diagnostic": str(parameters.session_dir / "failure_diagnostic.json") + if session.phase == Phase.FAILED and host.finalization.started else "", "publication_pointer": str(parameters.session_dir.parent/profile.artifacts.publication_pointer) - if session.phase == Phase.COMPLETE else ""}) + if session.phase == Phase.COMPLETE else "", + **({"diagnostic_report": + str(parameters.session_dir / "diagnostic_capture.json") + if session.phase == Phase.DIAGNOSTIC_COMPLETE else ""} if host.diagnostic_capture else {})}) devices = None if host.started else host._device_status(host.ports.monotonic()) preflight = host._camera_preflight if devices is None else CameraPreflight.from_readiness(devices.cameras) + diagnostic = None + if host.diagnostic_capture is not None: + diagnostic = {**host.diagnostic_capture.as_dict(), "publication_allowed": False, + "accuracy_validated": False, "coverage_scope": "raw_observations", + "unresolved_joint_zero_references": sorted(host.execution.diagnostic_zero_attempts), + "confirmed_joint_zero_references": sorted(host.execution.zero_references)} + if diagnostic_quality is not None: + diagnostic["accepted_pose_quality"] = diagnostic_quality.accepted_pose_report() return RuntimeSnapshot(status, profile.key.model, profile.key.side, profile.key.profile_id, parameters.serial_number, host.reason, str(parameters.session_dir), dict(host.reference_lock.latest_drift_px), - devices, steady, steady_node, preflight) + devices, steady, steady_node, preflight, + diagnostic) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py index f4dcb8b..65094f6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -6,6 +6,7 @@ from typing import Any, Mapping from ..core.domain.profile import CalibrationProfile from ..core.domain.status import CalibrationStatus +from .diagnostic_capture import diagnostic_capture_label from .reporting.reasons_zh import reason_zh @@ -20,6 +21,7 @@ _PHASES = { "FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "第四轮独立验证", "BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF", "PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停", + "DIAGNOSTIC_COMPLETE": "诊断采集完成（未进行精度验收）", "ABORTED": "已中止", "FAILED": "失败", "RUNNING": "标定中", "FINALIZING": "拟合与产物校验", "FITTING": "拟合与验证", "baseline": "安全恢复基准", "preflight": "固定通道映射点动", @@ -96,7 +98,7 @@ def normalize_status( if progress is None: count = int(value.get("step_count", 0) or 0) progress = (int(value.get("step_index", 0) or 0) + float(value.get("step_fraction", 0) or 0)) / count if count else 0.0 - result["overall_progress_01"] = 1.0 if state == "COMPLETE" else min(1.0, max(0.0, float(progress))) + result["overall_progress_01"] = 1.0 if state in {"COMPLETE", "DIAGNOSTIC_COMPLETE"} else min(1.0, max(0.0, float(progress))) result["phase"] = str(row.get("phase") or state) result["missing_tag_ids"] = value.get("missing_tag_ids", value.get("unrecognized_tag_ids", ())) result["hardware"] = { @@ -125,15 +127,42 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str: return "未知" if item is None else f"{float(item):.3f} {unit}" def tags(ids): return "/".join(f"ID{int(item)}" for item in ids) or "无" + phase = str(value.get("phase")) + phase_label = _PHASES.get(phase, value.get("phase")) + diagnostic_info = value.get("diagnostic_capture") or {} + diagnostic = bool(diagnostic_info) + sample_label = "原始观测" if diagnostic_info.get("coverage_scope") == "raw_observations" else "有效样本" + if diagnostic and phase in {"SWEEP", "sweep"}: + phase_label = "诊断扫描" + if diagnostic_info.get("hold_command_values"): + if phase == "steady_prepare": + phase_label = "方向差保持采集起点准备" + elif phase == "steady": + phase_label = f"方向差保持采集（每点至少 {diagnostic_info['hold_seconds']:g} 秒）" + capture_stage = task.get("capture_stage", "motion") + stage_line = f"阶段：{phase_label}（第 {task.get('cycle') or '-'}/{task.get('cycle_count', 4)} 轮）" + if capture_stage in {"command_training", "command_validation"}: + stage_line = "阶段：" + ("稳态指令映射训练" if capture_stage == "command_training" else "稳态指令映射独立验证") lines = [ f"[{value.get('model', '未知型号')} / {value.get('side', '?')} / {value.get('serial_number', '?')}] {progress:6.1%} 预计剩余 {eta_text}", - f"阶段：{_PHASES.get(str(value.get('phase')), value.get('phase'))}（第 {task.get('cycle') or '-'}/{task.get('cycle_count', 4)} 轮）", + stage_line, f"任务：{task.get('label') or task.get('key') or '无'}；方向：{task.get('direction') or '-'}", f"命令/反馈：{angle(motion.get('command'))}/{angle(motion.get('feedback'))}", "任务所需 Tag：" + ("；".join(f"{view}={tags(ids)}" for view, ids in task.get("required_tag_ids_by_view", {}).items()) or "无"), f"Tag：已识别 {tags(value.get('recognized_tag_ids', ()))}；缺失 {tags(value.get('missing_tag_ids', ()))}；过滤 {tags(value.get('rejected_tag_ids', ()))}", - f"有效样本 {acquisition.get('valid_samples', 0)}；覆盖 {float(acquisition.get('coverage_01', 0)):.1%}；分箱 {acquisition.get('bins', 0)}；内部空白 {acquisition.get('maximum_gap', 0)}；重扫 {acquisition.get('retry_count', 0)} 次", + f"{sample_label} {acquisition.get('valid_samples', 0)}；覆盖 {float(acquisition.get('coverage_01', 0)):.1%}；分箱 {acquisition.get('bins', 0)}；内部空白 {acquisition.get('maximum_gap', 0)}；重扫 {acquisition.get('retry_count', 0)} 次", ] + if diagnostic: + label = diagnostic_capture_label(diagnostic_info.get("mode", "o6_thumb")) + lines.insert(1, f"模式：{label}；仅采集观测，不发布标定 JSON/URDF") + accepted = diagnostic_info.get("accepted_pose_quality", {}) + metrics = tuple(accepted.get("metrics", {}).values()) + if metrics: + lines.append(f"可信姿态样本 {min(m['valid_frames'] for m in metrics)}；" + f"覆盖 {min(m['feedback_span'] for m in metrics):.1%}（未进行精度验收）") + unresolved = diagnostic_info.get("unresolved_joint_zero_references", ()) + if unresolved: + lines.append("零位未确认，仅保留原始观测：" + ", ".join(unresolved)) waiting = value.get("devices", {}).get("waiting_for", ()) if waiting: lines.append("等待项：" + "；".join(waiting)) @@ -163,7 +192,8 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str: resume = value.get("resume", {}) if resume: message = resume.get('message', resume.get('reason', '')) - message = {"not_requested": "本次完整重采，未请求恢复"}.get(message, message) + message = {"not_requested": "本次完整重采，未请求恢复", + "diagnostic_capture_no_resume": "诊断采集，不复用断点"}.get(message, message) lines.append(f"断点：{message}；复用 {resume.get('completed_unit_count', 0)} 单元；来源 {resume.get('source_session') or '-'}") hardware = value.get("hardware", {}) lines.append(f"SDK：反馈 {hardware.get('feedback_hz', '未知')} Hz；模式确认 {hardware.get('mode_verified', '未知')}；故障 {hardware.get('faults', '未知')}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py index aa3abbb..1af09c2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py @@ -3,10 +3,12 @@ import math -def steady_targets(profile, unit): - from ..core.domain.sampling import command_nodes +def steady_targets(profile, unit, *, records=(), training_nodes=None): + from ..core.domain.sampling import command_nodes, command_holdout_cycle task = next(t for t in profile.motion.tasks if t.key == unit.task_key) - values = command_nodes(profile, task, holdout=unit.cycle == 3) + from ..core.fitting.command_sampling import declared_training_nodes + values = (command_nodes(profile, task, holdout=True) if unit.cycle == command_holdout_cycle(profile) + else training_nodes if training_nodes is not None else declared_training_nodes(profile, task, records)) return tuple(values if unit.direction == "increasing" else reversed(values)) @@ -18,7 +20,7 @@ def steady_failures(profile, unit, attempt, records): and r.get("task_name") == unit.task_key and r.get("cycle") == unit.cycle and r.get("direction") == unit.direction and int(r.get("attempt", 1)) == attempt] for field, name, spec in observation_streams(profile, task): - for index, target in enumerate(steady_targets(profile, unit)): + for index, target in enumerate(steady_targets(profile, unit, records=records)): images = {r.get("image_stamp_ns") for r in selected if r.get("sample_phase") == "steady" and r.get(field) == name and r.get("view") == spec.view and r.get("task_name") == unit.task_key diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py new file mode 100644 index 0000000..d6d2164 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py @@ -0,0 +1,29 @@ +"""One bounded recovery per unreferenced joint group, without new zero claims.""" + + +class ZeroRecovery: + def __init__(self): + self._used = set() + + def take(self, motion, *, geometry_error, reports, referenced_joints): + if not motion.zero_joints or set(motion.zero_joints) & set(referenced_joints): + return None + if geometry_error: + # A partially installed multi-view model must not be refitted. + # Faults, changed holding poses and corrupt input are not retries. + if any(report.get("status") == "resolved" for report in reports.values()): + return None + reasons = {report.get("reason", "") for report in reports.values()} + recoverable = {"image_motion_insufficient_independent_frames", "image_motion_no_observable_image_model", + "image_motion_validation_frame_unresolved", "image_motion_families_not_distinguishable", + "image_motion_incomplete_candidate_family", "motion_geometry_unresolved"} + if not reasons or not reasons <= recoverable: + return None + action = "repeat_approach" + else: + action = "retry_zero_images" + key = (motion.task_key, tuple(motion.zero_joints)) + if key in self._used: + return None + self._used.add(key) + return action diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index 6bf6905..8904e62 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -52,12 +52,15 @@ setup( license="MIT", entry_points={ "console_scripts": [ + "rebuild_calibrated_urdf = linkerhand_calibration.runtime.artifacts.urdf_from_json:main", + "analyze_calibration_capture = linkerhand_calibration.runtime.diagnostic_analysis:main", "compare_calibration_urdfs = linkerhand_calibration.urdf_comparison:main", ( "hikrobot_camera_node = " "linkerhand_calibration.hikrobot_camera:main" ), "calibration_node = linkerhand_calibration.runtime.ros.entrypoint:main", + "tag_border_filter_node = linkerhand_calibration.runtime.ros.tag_border_filter:main", "o12_sdk_bridge = linkerhand_calibration.runtime.adapters.o12_bridge:main", ( "three_camera_calibration_node = " diff --git a/src/linkerhand_calibration/test/engine_capture_fixture.py b/src/linkerhand_calibration/test/engine_capture_fixture.py index 5d4de0e..a0417fd 100644 --- a/src/linkerhand_calibration/test/engine_capture_fixture.py +++ b/src/linkerhand_calibration/test/engine_capture_fixture.py @@ -17,6 +17,7 @@ from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapt from linkerhand_calibration.runtime.execution import SessionExecution from linkerhand_calibration.runtime.reference_lock import ReferenceLock from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from pose_branch_fixture import IdealPoseTracker def collect_with_engine(profile, observations): @@ -38,11 +39,12 @@ def collect_with_engine(profile, observations): key = (row["task_name"], row["cycle"], row["direction"], row.get("sample_phase", "sweep"), row.get("steady_index")) frame_groups[key][row["sample_id"]].append((row, spec)) poses = {view: {} for view in profile.vision.view_names} - class IdealPoseSensor: - maximum_reprojection_error_px = 1.5 - def __init__(self, view): self.view = view - def reset(self): pass - def estimate(self, role, corners, **kwargs): return poses[self.view][role], "" + class IdealPoseSensor(IdealPoseTracker): + def __init__(self, view): + super().__init__() + self.view = view + def estimate(self, role, corners, **kwargs): + return self.accept_ideal_pose(role, poses[self.view][role], kwargs["stamp_ns"]) def camera_pose(payload): value = camera_inverse @ transform_matrix(payload["translation_xyz_m"], payload["quaternion_xyzw"]) return SquareTagPose(tuple(Rotation.from_matrix(value[:3, :3]).as_quat()), tuple(value[:3, 3]), 0.) diff --git a/src/linkerhand_calibration/test/fixtures/camera_clock_receipts.json.gz b/src/linkerhand_calibration/test/fixtures/camera_clock_receipts.json.gz new file mode 100644 index 0000000..7a74329 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/camera_clock_receipts.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_final_image_sources_160040.json.gz b/src/linkerhand_calibration/test/fixtures/o6_final_image_sources_160040.json.gz new file mode 100644 index 0000000..a8c9784 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_final_image_sources_160040.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_20260914_175815_raw_replay.json.gz b/src/linkerhand_calibration/test/fixtures/o6_front_20260914_175815_raw_replay.json.gz new file mode 100644 index 0000000..dc20bf7 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_front_20260914_175815_raw_replay.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_20260914_192637_raw_replay.json.gz b/src/linkerhand_calibration/test/fixtures/o6_front_20260914_192637_raw_replay.json.gz new file mode 100644 index 0000000..33963fa Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_front_20260914_192637_raw_replay.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_161416_quality_events.json b/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_161416_quality_events.json new file mode 100644 index 0000000..61b44e0 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_161416_quality_events.json @@ -0,0 +1,474 @@ +{ + "source_session": "O6_RIGHT_001/20260914_161416", + "source_raw_sha256": "3eefc417ab968b54879241855336bf9ae5a5b5d31d8ae41c2915eac109afa912", + "scope": "Recorded frozen reference and five deformed front ID0 detections. No images after the pause were recorded; this fixture cannot prove actual recovery.", + "view": "front", + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "camera_matrix": [ + [ + 3792.732421875, + 0.0, + 606.0140166893543 + ], + [ + 0.0, + 3825.36962890625, + 568.5244182036549 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "reference_corners_xy": [ + [ + 651.6114807128906, + 852.3683776855469 + ], + [ + 708.6465759277344, + 845.8826293945312 + ], + [ + 701.8229675292969, + 787.6893920898438 + ], + [ + 644.96826171875, + 794.0107421875 + ] + ], + "reference_pose": { + "rotation_xyzw": [ + 0.05122618750941407, + 0.07257853631173206, + 0.05376952559779321, + -0.9945939231148989 + ], + "translation_xyz_m": [ + 0.019507045482571185, + 0.06870574180366773, + 1.0452219935270852 + ] + }, + "events": [ + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373674703790511, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 650.7820434570314, + 849.603271484375 + ], + [ + 708.2871704101564, + 846.6178588867189 + ], + [ + 701.3147583007812, + 787.5760498046876 + ], + [ + 644.46142578125, + 793.9281005859375 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.0689494439189896, + 0.0892766701000514, + -0.052421998914305996, + 0.9922336339738425 + ], + "translation_xyz_m": [ + 0.01927051064753879, + 0.0683358712786946, + 1.04173012169382 + ], + "reprojection_error_px": 0.8203549082285015 + }, + { + "quaternion_xyzw": [ + -0.13467791812542612, + -0.07440102998631523, + -0.04520090653912901, + 0.987057862110635 + ], + "translation_xyz_m": [ + 0.019286540244008814, + 0.0683040199267154, + 1.0416235980482782 + ], + "reprojection_error_px": 0.8884351664530162 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.8203549082285015, + "minimum_candidate_tilt_deg": 12.9537577740523, + "maximum_candidate_tilt_deg": 17.701656618264686, + "pose_constraint": "fixed_reference", + "fixed_reference_reprojection_error_px": 1.5511578554957017, + "fixed_reference_verified": false, + "observation_stamp_ns": 1789373674703790511, + "branch_revision": 2, + "branch_status": "fixed_reference_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 1, + "branch_frozen": true + }, + "branch_corrections_total": 1 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373674737107853, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 650.986572265625, + 849.4489135742189 + ], + [ + 708.4765625, + 846.6978759765625 + ], + [ + 701.5152587890625, + 787.5821533203124 + ], + [ + 644.6348266601564, + 793.9254150390625 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.07274616669000304, + 0.09173097100644584, + -0.05204806881576712, + 0.9917582481243471 + ], + "translation_xyz_m": [ + 0.019305954335328467, + 0.06827132134840262, + 1.0408129484230706 + ], + "reprojection_error_px": 0.8723269389119234 + }, + { + "quaternion_xyzw": [ + -0.1384302855795689, + -0.07679770133531316, + -0.044595240947531964, + 0.9863824986225185 + ], + "translation_xyz_m": [ + 0.019322228608171357, + 0.0682377527103236, + 1.0406931830626773 + ], + "reprojection_error_px": 0.944197809557915 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.8723269389119234, + "minimum_candidate_tilt_deg": 13.446654725220979, + "maximum_candidate_tilt_deg": 18.217183252762826, + "pose_constraint": "fixed_reference", + "fixed_reference_reprojection_error_px": 1.573755927601701, + "fixed_reference_verified": false, + "observation_stamp_ns": 1789373674737107853, + "branch_revision": 2, + "branch_status": "fixed_reference_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 2, + "branch_frozen": true + }, + "branch_corrections_total": 1 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373674770184767, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.1397705078125, + 849.4150390625003 + ], + [ + 708.6453247070312, + 846.7749633789064 + ], + [ + 701.6513061523439, + 787.606018066406 + ], + [ + 644.8040161132812, + 793.9556884765625 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.07377523792253451, + 0.09322053207746203, + -0.051915478601721, + 0.9915502658716917 + ], + "translation_xyz_m": [ + 0.01934209599907669, + 0.068254351936264, + 1.040451517901641 + ], + "reprojection_error_px": 0.9029827200918235 + }, + { + "quaternion_xyzw": [ + -0.1394470128719977, + -0.07824625765923164, + -0.04434376576803557, + 0.9861367472115171 + ], + "translation_xyz_m": [ + 0.0193585945536293, + 0.06822023556286914, + 1.040326908596307 + ], + "reprojection_error_px": 0.975293447689657 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.9029827200918235, + "minimum_candidate_tilt_deg": 13.65513087280534, + "maximum_candidate_tilt_deg": 18.402160681532425, + "pose_constraint": "fixed_reference", + "fixed_reference_reprojection_error_px": 1.5711497009701978, + "fixed_reference_verified": false, + "observation_stamp_ns": 1789373674770184767, + "branch_revision": 2, + "branch_status": "fixed_reference_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 3, + "branch_frozen": true + }, + "branch_corrections_total": 1 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373674803781247, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.1066894531251, + 849.4465332031251 + ], + [ + 708.6222534179689, + 846.8680419921876 + ], + [ + 701.619140625, + 787.6516723632811 + ], + [ + 644.7723388671875, + 794.01220703125 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.07416052126135776, + 0.09401877076851597, + -0.05184184633608467, + 0.9914500041847227 + ], + "translation_xyz_m": [ + 0.019326739165784403, + 0.06824498811606222, + 1.0400731042057596 + ], + "reprojection_error_px": 0.9209144087717472 + }, + { + "quaternion_xyzw": [ + -0.13983752470383345, + -0.07905290919387553, + -0.04421050999107296, + 0.9860230905202062 + ], + "translation_xyz_m": [ + 0.019343352432883133, + 0.06821053937063642, + 1.0399448592973592 + ], + "reprojection_error_px": 0.9939407301090831 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.9209144087717472, + "minimum_candidate_tilt_deg": 13.754987577665794, + "maximum_candidate_tilt_deg": 18.487617322056742, + "pose_constraint": "fixed_reference", + "fixed_reference_reprojection_error_px": 1.5736869605739345, + "fixed_reference_verified": false, + "observation_stamp_ns": 1789373674803781247, + "branch_revision": 2, + "branch_status": "fixed_reference_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 4, + "branch_frozen": true + }, + "branch_corrections_total": 1 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373674837065731, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_conflict", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 650.91162109375, + 849.4388427734373 + ], + [ + 708.4180297851561, + 846.9037475585938 + ], + [ + 701.4273681640625, + 787.6770629882815 + ], + [ + 644.5805053710939, + 794.0317382812501 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.0745974181611575, + 0.09433986520903168, + -0.051717641242694276, + 0.9913932119095445 + ], + "translation_xyz_m": [ + 0.019272532256405614, + 0.06824822341509994, + 1.040046126872147 + ], + "reprojection_error_px": 0.9295222058852461 + }, + { + "quaternion_xyzw": [ + -0.14027036228299425, + -0.07941984067154015, + -0.04406108200999516, + 0.9859388091686081 + ], + "translation_xyz_m": [ + 0.019289181266401056, + 0.068213529376978, + 1.0399155768327384 + ], + "reprojection_error_px": 1.0031381095366472 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.9295222058852461, + "minimum_candidate_tilt_deg": 13.81532839490434, + "maximum_candidate_tilt_deg": 18.55233483567943, + "pose_constraint": "fixed_reference", + "fixed_reference_reprojection_error_px": 1.6233466461226242, + "fixed_reference_verified": false, + "observation_stamp_ns": 1789373674837065731, + "branch_revision": 2, + "branch_status": "frozen_conflict", + "branch_reason": "pose_branch_conflict", + "branch_confirmation_count": 5, + "branch_frozen": true + }, + "branch_corrections_total": 1 + } + } + ] +} diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_branch_events.json b/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_branch_events.json new file mode 100644 index 0000000..910924c --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_front_base_20260914_branch_events.json @@ -0,0 +1,459 @@ +{ + "source_session": "O6_RIGHT_001/20260914_160325", + "source_raw_sha256": "e08be16c6c77f031cdc524c89fdeb103f2a5cf2c3c8bb67187f6b5db570f640f", + "scope": "Recorded frozen reference and five conflicting front ID0 images; successful preparation images were not recorded. Not a full session or accuracy replay.", + "view": "front", + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "reference_corners_xy": [ + [ + 651.3810424804688, + 852.2985229492188 + ], + [ + 708.4353942871094, + 845.8116455078125 + ], + [ + 701.5877380371094, + 787.6119079589844 + ], + [ + 644.7322692871093, + 793.9474182128906 + ] + ], + "reference_pose": { + "rotation_xyzw": [ + 0.051657382830113, + 0.07233818005965809, + 0.053787910495170524, + -0.9945881374666417 + ], + "translation_xyz_m": [ + 0.019443198546650248, + 0.06867587597205813, + 1.04523215512729 + ] + }, + "camera_matrix": [ + [ + 3792.732421875, + 0.0, + 606.0140166893543 + ], + [ + 0.0, + 3825.36962890625, + 568.5244182036549 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "events": [ + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373017897501233, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.2639160156251, + 851.8523559570312 + ], + [ + 708.3605346679686, + 845.7075195312501 + ], + [ + 701.5051879882811, + 787.6116333007814 + ], + [ + 644.6477050781251, + 793.9578247070312 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + -0.0017637893722219381, + 0.0741246018891116, + -0.058492815624425876, + 0.9955305233704078 + ], + "translation_xyz_m": [ + 0.019470355602835353, + 0.06889911011030934, + 1.0489767358449047 + ], + "reprojection_error_px": 0.05631830982299966 + }, + { + "quaternion_xyzw": [ + -0.06454724733730362, + -0.05935707358630751, + -0.05353332191340803, + 0.9947082859418444 + ], + "translation_xyz_m": [ + 0.01948586951276129, + 0.06889041042895316, + 1.0489728093762585 + ], + "reprojection_error_px": 0.08873372663232251 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.05631830982299966, + "minimum_candidate_tilt_deg": 8.504262310381518, + "maximum_candidate_tilt_deg": 10.061504700361334, + "observation_stamp_ns": 1789373017897501233, + "branch_revision": 1, + "branch_status": "conflict_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 1, + "branch_frozen": true + }, + "branch_corrections_total": 0 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373017930878824, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.2874145507811, + 851.705810546875 + ], + [ + 708.3966674804688, + 845.7094116210938 + ], + [ + 701.5343017578125, + 787.6038208007812 + ], + [ + 644.6768798828125, + 793.9531250000001 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.003890939029632483, + 0.0723875846158807, + -0.0582059228393556, + 0.99566910604546 + ], + "translation_xyz_m": [ + 0.019486955902090344, + 0.06891798815024941, + 1.0494180790291907 + ], + "reprojection_error_px": 0.08163328008782858 + }, + { + "quaternion_xyzw": [ + -0.07018371470015455, + -0.05760421277028976, + -0.05325691004080239, + 0.994444418957092 + ], + "translation_xyz_m": [ + 0.019501897842959144, + 0.06890773159582059, + 1.049410573675387 + ], + "reprojection_error_px": 0.12399867839301451 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.08163328008782858, + "minimum_candidate_tilt_deg": 8.314273514007814, + "maximum_candidate_tilt_deg": 10.418858251220792, + "observation_stamp_ns": 1789373017930878824, + "branch_revision": 1, + "branch_status": "conflict_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 2, + "branch_frozen": true + }, + "branch_corrections_total": 0 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373017964203500, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.2019653320314, + 851.4394531249999 + ], + [ + 708.349365234375, + 845.7827758789062 + ], + [ + 701.4730834960938, + 787.6358032226564 + ], + [ + 644.6278686523438, + 793.9785156249999 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.015547389407951088, + 0.07097620670743443, + -0.05751186024699874, + 0.9956972645814344 + ], + "translation_xyz_m": [ + 0.019476327145696467, + 0.06893103415003812, + 1.0497373401099155 + ], + "reprojection_error_px": 0.15922816946232976 + }, + { + "quaternion_xyzw": [ + -0.08179270149397419, + -0.0561873622981887, + -0.05244234274300198, + 0.993681405173668 + ], + "translation_xyz_m": [ + 0.019490603849034997, + 0.0689175370110156, + 1.049722140431075 + ], + "reprojection_error_px": 0.2064577346482914 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.15922816946232976, + "minimum_candidate_tilt_deg": 8.333461835216385, + "maximum_candidate_tilt_deg": 11.38993872662255, + "observation_stamp_ns": 1789373017964203500, + "branch_revision": 1, + "branch_status": "conflict_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 3, + "branch_frozen": true + }, + "branch_corrections_total": 0 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373017997253983, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_unconfirmed", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.0892944335936, + 851.2770996093753 + ], + [ + 708.2705688476564, + 845.88818359375 + ], + [ + 701.3729858398438, + 787.6652832031248 + ], + [ + 644.5391845703123, + 794.0059814453125 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.023056471882254036, + 0.07254119338325239, + -0.05704579812544366, + 0.9954656956837376 + ], + "translation_xyz_m": [ + 0.019440361452008358, + 0.06889820121116787, + 1.0492236406616988 + ], + "reprojection_error_px": 0.22416292552376008 + }, + { + "quaternion_xyzw": [ + -0.0892589805271742, + -0.05776484561035559, + -0.05173711763178937, + 0.9929850591353404 + ], + "translation_xyz_m": [ + 0.019454762778121373, + 0.06888246787152717, + 1.0492014615765388 + ], + "reprojection_error_px": 0.27421207829911554 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.22416292552376008, + "minimum_candidate_tilt_deg": 8.7308314332582, + "maximum_candidate_tilt_deg": 12.206445788097424, + "observation_stamp_ns": 1789373017997253983, + "branch_revision": 1, + "branch_status": "conflict_pending", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 4, + "branch_frozen": true + }, + "branch_corrections_total": 0 + } + }, + { + "kind": "pnp_branch_event", + "view": "front", + "image_stamp_ns": 1789373018030961996, + "tag_role": "front_base", + "tag_id": 0, + "reason": "pose_branch_conflict", + "pnp_observation_evidence": { + "tag_role": "front_base", + "tag_id": 0, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 651.0183715820312, + 851.0774536132814 + ], + [ + 708.2586669921877, + 846.02001953125 + ], + [ + 701.3419189453124, + 787.6656494140623 + ], + [ + 644.4960327148436, + 794.0057373046875 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + 0.030940978354305437, + 0.07516598998187739, + -0.05644175675597389, + 0.9950914821778057 + ], + "translation_xyz_m": [ + 0.019406210239914932, + 0.06881287778423942, + 1.0479804729911537 + ], + "reprojection_error_px": 0.3049068030194769 + }, + { + "quaternion_xyzw": [ + -0.0970848585178656, + -0.06038259334163152, + -0.05081850378140816, + 0.9921421028972232 + ], + "translation_xyz_m": [ + 0.019420932377868627, + 0.06879463727498322, + 1.04794854719117 + ], + "reprojection_error_px": 0.3585928916271403 + } + ], + "candidate_diagnostics": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.3049068030194769, + "minimum_candidate_tilt_deg": 9.324876196298892, + "maximum_candidate_tilt_deg": 13.130053999046243, + "observation_stamp_ns": 1789373018030961996, + "branch_revision": 1, + "branch_status": "frozen_conflict", + "branch_reason": "pose_branch_conflict", + "branch_confirmation_count": 5, + "branch_frozen": true + }, + "branch_corrections_total": 0 + } + } + ] +} diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.json b/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.json new file mode 100644 index 0000000..7de054f --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.json @@ -0,0 +1,60 @@ +{ + "source": "calibration_output/O6_RIGHT_001/20260915_121857/raw_samples.jsonl", + "source_sha256": "3f89b5010ff5b225a49233bc6020467f8f3445f844348d4fd0db2ddd1d51957c", + "image_topic": "/o6_calibration/front/camera/image_rect", + "crop_origin_xy": [ + 600, + 730 + ], + "purpose": "fixed Tag border occlusion regression, no geometry certification", + "commands": [ + [ + 255.0, + 255.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + [ + 255.0, + 254.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + [ + 255.0, + 23.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + [ + 255.0, + 20.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + [ + 255.0, + 15.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + [ + 255.0, + 12.0, + 255.0, + 255.0, + 255.0, + 255.0 + ] + ] +} \ No newline at end of file diff --git a/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.npz b/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.npz new file mode 100644 index 0000000..71fe30a Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_front_border_121857.npz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_image_live_flow_200044.json.gz b/src/linkerhand_calibration/test/fixtures/o6_image_live_flow_200044.json.gz new file mode 100644 index 0000000..17d767e Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_image_live_flow_200044.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_image_motion_200044.json.gz b/src/linkerhand_calibration/test/fixtures/o6_image_motion_200044.json.gz new file mode 100644 index 0000000..df5a540 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_image_motion_200044.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_144514.json.gz b/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_144514.json.gz new file mode 100644 index 0000000..6cabc65 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_144514.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_160040.json.gz b/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_160040.json.gz new file mode 100644 index 0000000..59f0366 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_pinky_preparation_160040.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_raw_replay_sources.md b/src/linkerhand_calibration/test/fixtures/o6_raw_replay_sources.md new file mode 100644 index 0000000..2a21c02 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_raw_replay_sources.md @@ -0,0 +1,22 @@ +# O6 原始观测回归切片 + +两个 `o6_front_20260914_*_raw_replay.json.gz` 文件分别来自不同会话；不拼接零位、安装关系或图像，不作为整只手精度验收。 +Gzip 只压缩 JSON，`mtime=0`；`records` 中每条记录与原始日志完全一致，未删字段或重算候选。 + +| 来源会话 | 保存内容 | 未提供的证据 | +| --- | --- | --- | +| `20260914_175815` | 正面拇指 pitch/IP 两个方向的 395 条原始观测、会话头、相机模型 | 新版归零准备运动；第四轮独立精度验收 | +| `20260914_192637` | 正面归零准备运动最终选用的 120 条原始帧、原初始化报告、会话头、相机模型 | 暂停后未发生的 pitch/IP 扫描；完整诊断采集；第四轮独立精度验收 | + +原始日志 SHA-256： + +- `175815`：`8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d` +- `192637`：`757ba1d7cccae4bdb93553b2735e94862bc74f57f153198e1a9d3eff76d45c8d` + +`192637` 的切片按原 `motion_branch_initialization.source_image_stamps` 选择，并保留每帧原始哈希。 +`175815` 的切片按 `kind=diagnostic_observation_frame`、`view=front`、`task_name=thumb_pitch_ip_front` 选择。 +测试重新计算原角点的 IPPE 候选，分别验证原始观测覆盖和已接受关节样本覆盖；原零位不被重拟合。 +完整本地日志存在时，还比较每条切片记录与原记录，并校验原日志哈希；缺少完整日志时，该额外核验显式跳过。 + +运行 `test_recorded_diagnostic_replay.py` 通过，只说明这些明确边界成立。 +新版完整采集流程使用另行标识的模拟时钟集成测试；不会把模拟完成解释为这两个真实会话完整通过。 diff --git a/src/linkerhand_calibration/test/fixtures/o6_side_base_20260914_190619.json b/src/linkerhand_calibration/test/fixtures/o6_side_base_20260914_190619.json new file mode 100644 index 0000000..42d0e79 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_side_base_20260914_190619.json @@ -0,0 +1,182 @@ +{ + "scope": "stationary_fixed_tag_ambiguity_regression_not_accuracy_certificate", + "source_raw_sha256": "43f7aa037caf28decf2e729735f26ffabe5fd6369179b130fa59d884ce54e4b6", + "camera_model": { + "kind": "rectified_camera_model", + "view": "side", + "matrix_source": "CameraInfo.P[:3,:3]", + "width": 1624, + "height": 1240, + "raw_k": [ + 3701.9669310172562, + 0.0, + 780.38448956403, + 0.0, + 3708.0764820855165, + 543.1934166854517, + 0.0, + 0.0, + 1.0 + ], + "raw_d": [ + -0.0855955821613016, + 1.7771811866066576, + -0.009882528169757824, + -0.0018744565873629446, + 0.0 + ], + "rectification_r": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "projection_p": [ + 3719.39794921875, + 0.0, + 779.2527216851595, + 0.0, + 0.0, + 3715.812255859375, + 538.8889945124574, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + "camera_matrix": [ + [ + 3719.39794921875, + 0.0, + 779.2527216851595 + ], + [ + 0.0, + 3715.812255859375, + 538.8889945124574 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "input_is_rectified": true + }, + "observation": { + "kind": "pnp_branch_event", + "view": "side", + "image_stamp_ns": 1789384038002424192, + "tag_role": "side_base", + "tag_id": 3, + "reason": "pose_branch_unconfirmed", + "task_name": null, + "motion_phase": null, + "command_unit": "u8", + "command_vector": [ + 255.0, + 255.0, + 255.0, + 255.0, + 255.0, + 255.0 + ], + "feedback_vector": [ + 254.0, + 255.0, + 254.0, + 254.0, + 254.0, + 254.0 + ], + "state_image_sync_error_ms": 7.491799, + "timing": { + "received_stamp_ns": 1789384038027697599, + "history_snapshot_stamp_ns": 1789384038027907158, + "latest_feedback_stamp_ns": 1789384038009915991, + "latest_command_stamp_ns": 1789384038027461363 + }, + "pnp_observation_evidence": { + "tag_role": "side_base", + "tag_id": 3, + "tag_size_m": 0.016, + "pose_source": "current_image", + "corners_xy": [ + [ + 635.7402954101564, + 871.7483520507814 + ], + [ + 715.5008544921875, + 872.2428588867189 + ], + [ + 716.2201538085936, + 792.0438842773436 + ], + [ + 636.2169799804688, + 791.7636718749999 + ] + ], + "selected_pose": null, + "reprojection_valid_candidates": [ + { + "quaternion_xyzw": [ + -0.044633764301794464, + 0.0076389447486132234, + 0.0021611953202176335, + 0.9989718728984152 + ], + "translation_xyz_m": [ + -0.020686883190194915, + 0.058714895217251775, + 0.744562855591804 + ], + "reprojection_error_px": 0.10935170446797024 + }, + { + "quaternion_xyzw": [ + -0.0341108338913104, + -0.03511406600381762, + 0.003990533332137572, + 0.9987930361309315 + ], + "translation_xyz_m": [ + -0.02067953829092587, + 0.05871663804033328, + 0.7445618482013543 + ], + "reprojection_error_px": 0.12294261238764193 + } + ], + "candidate_diagnostics": { + "motion_managed": false, + "motion_selection_committed": false, + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + "maximum_reprojection_error_px": 1.5, + "maximum_independent_tilt_deg": 75.0, + "minimum_reprojection_error_px": 0.10935170446797024, + "minimum_candidate_tilt_deg": 5.190794593307594, + "maximum_candidate_tilt_deg": 5.612019732803669, + "observation_stamp_ns": 1789384038002424192, + "branch_revision": 0, + "branch_status": "initializing", + "branch_reason": "pose_branch_unconfirmed", + "branch_confirmation_count": 0, + "branch_frozen": false, + "motion_evidence_verified": false, + "motion_evidence_id": "" + }, + "branch_corrections_total": 0 + } + } +} diff --git a/src/linkerhand_calibration/test/fixtures/o6_side_preparation_132102.json.gz b/src/linkerhand_calibration/test/fixtures/o6_side_preparation_132102.json.gz new file mode 100644 index 0000000..4db3591 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_side_preparation_132102.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_side_preparation_193254.json.gz b/src/linkerhand_calibration/test/fixtures/o6_side_preparation_193254.json.gz new file mode 100644 index 0000000..55e3d19 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_side_preparation_193254.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.json.gz b/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.json.gz new file mode 100644 index 0000000..5c75426 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.metadata.json b/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.metadata.json new file mode 100644 index 0000000..b57e2e3 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_spatial_125641.metadata.json @@ -0,0 +1,7 @@ +{ + "session": "20260915_125641", + "purpose": "Spatial solve order regression; numerical solver input extracted from recorded measurements, no publication authority.", + "raw_samples_sha256": "1766ea47365115ada9783ef434e6b19a85b9473c6d7ec212bb6a8b90c2a14fae", + "profile_sha256": "ef38a4815cc595653583fa70b679bee1f988d7f90a6cc9e223738f2682e2f710", + "source_urdf_sha256": "8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62" +} diff --git a/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_175815_motion_evidence.json b/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_175815_motion_evidence.json new file mode 100644 index 0000000..56f4490 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_175815_motion_evidence.json @@ -0,0 +1 @@ +{"scope":"single_round_counterfactual_branch_regression_not_accuracy_certificate","source_raw_sha256":"8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d","source_session":"O6_RIGHT_001/20260914_175815","source_phase":"sweep_after_original_zero_freeze","usage":"offline counterfactual only; never import these frames as production zero-approach evidence or rewrite original frozen references","frames":[{"stamp_ns":1789379968738078329,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06620146283990917}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024562752928947124,-0.34062211631533607,-0.1409323753649647,0.9292530928758871],"translation_xyz_m":[0.06717955034158052,0.10216267558904864,1.0191361154256278],"reprojection_error_px":0.15510217144789182},{"quaternion_xyzw":[-0.12606227997358202,0.3849685346768518,-0.17221243050681864,0.8979144768047533],"translation_xyz_m":[0.0671163456053571,0.10215902643030156,1.0191235599365087],"reprojection_error_px":0.21507449743549475}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0020246340816874307,0.32790236507252807,-0.2106886020838658,0.9209159857384517],"translation_xyz_m":[0.09972952214560166,0.08532429501742125,0.9890056434198886],"reprojection_error_px":0.14446111573290465},{"quaternion_xyzw":[-0.0978446429141171,-0.2509904401971761,-0.1810067500108367,0.9458682684354894],"translation_xyz_m":[0.09978228745045173,0.08530026509111677,0.9889758036496685],"reprojection_error_px":0.22686352141149865}]}]},{"stamp_ns":1789379968771229475,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10700670428844448}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02460752742981595,-0.3407403835863257,-0.14092964223145737,0.9292089627891822],"translation_xyz_m":[0.06718624322369554,0.10215056031367306,1.0191423336760697],"reprojection_error_px":0.1558257900089534},{"quaternion_xyzw":[-0.12609141201830448,0.38509101568695864,-0.17221426268975032,0.8978575127372062],"translation_xyz_m":[0.06712305529302322,0.10214693751012031,1.0191300542625135],"reprojection_error_px":0.21501480188549452}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0014558847099267471,0.32767059929067016,-0.2107510587812212,0.9209852604590362],"translation_xyz_m":[0.0997486877066254,0.0853225591964265,0.9891243698614641],"reprojection_error_px":0.1325963295507424},{"quaternion_xyzw":[-0.09840963164281312,-0.25075277322434497,-0.1810359933648082,0.9458670943777688],"translation_xyz_m":[0.09980083871233052,0.08529795152345711,0.9890890877990207],"reprojection_error_px":0.23403843488166542}]}]},{"stamp_ns":1789379968804677380,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05827984394845609}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0245978590596999,-0.34082268550343503,-0.14090114931223807,0.9291833556938027],"translation_xyz_m":[0.06717953657392466,0.10216073237017147,1.0191922010766248],"reprojection_error_px":0.15676724830637384},{"quaternion_xyzw":[-0.12607956914903648,0.38516483287974007,-0.17219632612494276,0.897830952365192],"translation_xyz_m":[0.06711624004180014,0.10215694432896481,1.0191782397148192],"reprojection_error_px":0.22011911410717055}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016057780736639113,0.3277078917826488,-0.21072467571089668,0.920977779420702],"translation_xyz_m":[0.09973320942202948,0.08532166536080117,0.9890655129855379],"reprojection_error_px":0.1310528427579344},{"quaternion_xyzw":[-0.09825911171762623,-0.2507981784160554,-0.18102055372916223,0.9458736595313557],"translation_xyz_m":[0.09978538059045243,0.08529708883423037,0.9890302595142896],"reprojection_error_px":0.23307570147183262}]}]},{"stamp_ns":1789379968838041077,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04222914617195462}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024475629529665876,-0.340703553818415,-0.1409798329388574,0.9292183374638927],"translation_xyz_m":[0.06715909556042733,0.10216350519047854,1.019131445966457],"reprojection_error_px":0.15497127983359762},{"quaternion_xyzw":[-0.12597384344910034,0.3850245689819556,-0.17227440580510336,0.8978909739781437],"translation_xyz_m":[0.06709586043286168,0.1021598326597372,1.0191184492596244],"reprojection_error_px":0.21607771902937384}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016827990187224598,0.32773003466182066,-0.21065931462882115,0.9209847152521803],"translation_xyz_m":[0.09972012413216585,0.08533163277040212,0.9890762110987198],"reprojection_error_px":0.14606099616043777},{"quaternion_xyzw":[-0.09818454848701794,-0.2508258102101196,-0.1809580404563042,0.945886036985985],"translation_xyz_m":[0.09977290558601586,0.08530758964540507,0.9890469273361268],"reprojection_error_px":0.22638105228826585}]}]},{"stamp_ns":1789379968871586471,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.050553206973934194}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024661547753132916,-0.34063487840257356,-0.14107734489462248,0.9292237999726265],"translation_xyz_m":[0.06715145918323143,0.1021638902171283,1.019054252794838],"reprojection_error_px":0.16143756651451557},{"quaternion_xyzw":[-0.12616987809606336,0.38494322715872203,-0.17235408708227426,0.8978830338033396],"translation_xyz_m":[0.06708828212239476,0.10216028244612238,1.0190421854991845],"reprojection_error_px":0.21957460373192295}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0017400322422331644,0.32796276487522075,-0.21069077370587336,0.9208945623785696],"translation_xyz_m":[0.09972298691239169,0.08534728391590629,0.9891335699836912],"reprojection_error_px":0.15062793429899116},{"quaternion_xyzw":[-0.09813261535635047,-0.2510717272254876,-0.18097141708059292,0.9458236219249205],"translation_xyz_m":[0.09977606632107601,0.085323483711475,0.9891070034559666],"reprojection_error_px":0.22199097365128376}]}]},{"stamp_ns":1789379968904707752,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.045130538312267626}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02464661722905223,-0.3405681041416748,-0.14101168029908948,0.9292586382271286],"translation_xyz_m":[0.06716809976254365,0.10216154980295808,1.0191827705185177],"reprojection_error_px":0.15701186437457929},{"quaternion_xyzw":[-0.12614366253157006,0.3848954309107791,-0.17227881664865335,0.8979216519280883],"translation_xyz_m":[0.06710494058046988,0.10215795308637793,1.0191708459517075],"reprojection_error_px":0.215178363583689}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0015694612646773712,0.3279173911887081,-0.21071096459563393,0.9209064071590055],"translation_xyz_m":[0.09973031622305559,0.08532798882795356,0.989130469188155],"reprojection_error_px":0.14119433370963322},{"quaternion_xyzw":[-0.09828872077870894,-0.25101927524999207,-0.1809845693826329,0.9458188179910764],"translation_xyz_m":[0.09978275875780901,0.08530362452007349,0.9890977360318437],"reprojection_error_px":0.23251148435173574}]}]},{"stamp_ns":1789379968938099077,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10556002934261874}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024590192527481906,-0.34069792018328426,-0.14087817661187077,0.9292327958960339],"translation_xyz_m":[0.06717612810834431,0.1021419436810182,1.0191324400930608],"reprojection_error_px":0.15432797469013218},{"quaternion_xyzw":[-0.1260660397677797,0.3850483352575379,-0.1721583085346365,0.8978901101666952],"translation_xyz_m":[0.06711296753133497,0.10213836062403267,1.019120574671737],"reprojection_error_px":0.2125773456652466}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016655161571982784,0.3280752810924436,-0.21066055365728778,0.9208615352618706],"translation_xyz_m":[0.09974425985690362,0.08531798170975252,0.9891699202930889],"reprojection_error_px":0.14592628030311702},{"quaternion_xyzw":[-0.09817462958100337,-0.2511626498771703,-0.18093513020488003,0.9458020638965925],"translation_xyz_m":[0.09979700330588771,0.08529388105357502,0.9891399696962094],"reprojection_error_px":0.22807438506639904}]}]},{"stamp_ns":1789379968971542973,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12474581878558229}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024640790172912028,-0.34080253964248713,-0.1410031012199785,0.9291741418481435],"translation_xyz_m":[0.06717225243982432,0.10212305979579045,1.0189915875858109],"reprojection_error_px":0.15536535019746087},{"quaternion_xyzw":[-0.12611549106669037,0.38514088802599017,-0.1722879530498448,0.8978186011202116],"translation_xyz_m":[0.06710919938193467,0.10211965985463406,1.0189815102771596],"reprojection_error_px":0.20881525877865204}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0013734915214916768,0.32789291279431126,-0.2107346838945732,0.920910008776187],"translation_xyz_m":[0.0997352367746075,0.08529911247822067,0.9890290897667643],"reprojection_error_px":0.12819516916391024},{"quaternion_xyzw":[-0.09846484599099896,-0.25097903371560815,-0.18099921250146175,0.9458083758420619],"translation_xyz_m":[0.09978778644359641,0.0852748092083401,0.9889975093720014],"reprojection_error_px":0.22133475902813762}]}]},{"stamp_ns":1789379969004763995,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0689808217039091}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02480823905959979,-0.3407015272857634,-0.14150556824867283,0.9291303432428252],"translation_xyz_m":[0.06716538236701383,0.1021317686898511,1.0190912875929627],"reprojection_error_px":0.15173858291867065},{"quaternion_xyzw":[-0.12630821705638812,0.384976626638313,-0.17276615120585914,0.8977700642390206],"translation_xyz_m":[0.0671022322240413,0.10212820887113401,1.0190796231740316],"reprojection_error_px":0.20966553346589445}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0025290327790482695,0.3276853707031337,-0.2117816612272289,0.9207412393219866],"translation_xyz_m":[0.09971180204760875,0.08522273963426885,0.9890908632371092],"reprojection_error_px":0.15535816070485933},{"quaternion_xyzw":[-0.09733650160353165,-0.25088865872529803,-0.18219640828684908,0.9457192792715914],"translation_xyz_m":[0.09976485061212734,0.08519900231155318,0.9890639992986482],"reprojection_error_px":0.22631814179762935}]}]},{"stamp_ns":1789379969038101811,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0664390925118391}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025029402417802497,-0.34066647115323473,-0.14184081214351932,0.9290861469514421],"translation_xyz_m":[0.0671545292976387,0.10216193542966877,1.0192317118858676],"reprojection_error_px":0.15907220908593614},{"quaternion_xyzw":[-0.12655320270188028,0.3848857687447632,-0.17308547258270401,0.8977130115379143],"translation_xyz_m":[0.06709130371263183,0.10215822773362421,1.0192187509351667],"reprojection_error_px":0.21965109778679356}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0027138121319543406,0.32739208691631855,-0.21265904301051294,0.9206428124273512],"translation_xyz_m":[0.099699744161964,0.08519550327263833,0.9892775306468843],"reprojection_error_px":0.14679697552209162},{"quaternion_xyzw":[-0.09718559288598438,-0.2507055904620155,-0.1831253765227051,0.9456039149241661],"translation_xyz_m":[0.0997523796596387,0.08517143701702375,0.9892470161513586],"reprojection_error_px":0.23041037719397367}]}]},{"stamp_ns":1789379969071994795,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09499117147502247}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024930028837555844,-0.3407351423818495,-0.14181795216937823,0.92906712612742],"translation_xyz_m":[0.06713670866480731,0.10214695451874527,1.0190530477080366],"reprojection_error_px":0.15453618126283533},{"quaternion_xyzw":[-0.1264547860972081,0.38494913345885196,-0.17307784888904326,0.8977011807646963],"translation_xyz_m":[0.06707344507622624,0.10214322463186064,1.0190396934792296],"reprojection_error_px":0.21660014752484724}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0029461477357522027,0.3273574290331233,-0.2127342703253217,0.9206370425414652],"translation_xyz_m":[0.0996881936155351,0.08519137514565327,0.9892473186134294],"reprojection_error_px":0.1484454244252896},{"quaternion_xyzw":[-0.09695859973581365,-0.2506850941904335,-0.1832270493981266,0.9456129556308227],"translation_xyz_m":[0.09974106811258715,0.08516754301529791,0.9892190691534593],"reprojection_error_px":0.2253889707108944}]}]},{"stamp_ns":1789379969104729178,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.047010857997014065}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025025002829715554,-0.3405804486503894,-0.14194422878675156,0.9291020090090252],"translation_xyz_m":[0.06715900789228779,0.10214648301537788,1.019206585808686],"reprojection_error_px":0.1558498204756077},{"quaternion_xyzw":[-0.1265483751787698,0.3847982349365504,-0.17317629471633939,0.8977337010927283],"translation_xyz_m":[0.06709586148143878,0.1021429010625322,1.019194839231218],"reprojection_error_px":0.2136410113642889}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0028150723403997865,0.327100818062349,-0.21307986328778833,0.920648739776424],"translation_xyz_m":[0.09969907519915706,0.08516077306339205,0.9892784168503883],"reprojection_error_px":0.14037608689871833},{"quaternion_xyzw":[-0.09709400156073528,-0.2504470302378267,-0.18359010828581082,0.9455917258762268],"translation_xyz_m":[0.0997515825683554,0.08513662089692707,0.9892469551818859],"reprojection_error_px":0.22883538749914226}]}]},{"stamp_ns":1789379969138060209,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09138829266146593}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025191099223906898,-0.3405213216608216,-0.14285289246869085,0.9289799185814445],"translation_xyz_m":[0.06715820380246613,0.1020903093850525,1.0192598632811156],"reprojection_error_px":0.1702049374790148},{"quaternion_xyzw":[-0.12669918005516065,0.38464842243957903,-0.17404054121905502,0.8976094913158347],"translation_xyz_m":[0.06709509222816998,0.1020868072094963,1.0192485953619275],"reprojection_error_px":0.22555057372968576}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0040297361049375525,0.3266329178069535,-0.21494466308302,0.9203768195929141],"translation_xyz_m":[0.09968338371127593,0.08497269091330827,0.989420580698134],"reprojection_error_px":0.143597645041166},{"quaternion_xyzw":[-0.09586403295820742,-0.2501582402017518,-0.1856691905337382,0.9453876949329819],"translation_xyz_m":[0.09973606914568671,0.08494883888853826,0.989391071952364],"reprojection_error_px":0.2258068714489241}]}]},{"stamp_ns":1789379969171491443,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10996728414496998}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025535328914461156,-0.3404065888445696,-0.14321965623181296,0.9289560976264033],"translation_xyz_m":[0.06715611644750238,0.10207752447353832,1.0192845898548324],"reprojection_error_px":0.1790384390617395},{"quaternion_xyzw":[-0.12704384617388054,0.3844974132916339,-0.17436443307940608,0.8975626133024298],"translation_xyz_m":[0.06709278621798023,0.10207364641371801,1.0192699426228817],"reprojection_error_px":0.24133505386627208}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.003432306366399863,0.32637216491992466,-0.2153793540375449,0.9203701228814735],"translation_xyz_m":[0.09968142211947512,0.08493935501038734,0.9895028036470392],"reprojection_error_px":0.1499680266797322},{"quaternion_xyzw":[-0.096460543017721,-0.249937574951695,-0.18607659859506198,0.945305279644245],"translation_xyz_m":[0.09973437873115575,0.0849156610305992,0.9894767234576709],"reprojection_error_px":0.22064129199327184}]}]},{"stamp_ns":1789379969204987745,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08069269512272567}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02533530882957882,-0.34039559803602903,-0.14359988288072,0.9289069020094788],"translation_xyz_m":[0.06715253957073487,0.10208448511827647,1.01930940947106],"reprojection_error_px":0.18144677392985104},{"quaternion_xyzw":[-0.12686818345763437,0.384439472695914,-0.17475548407430103,0.8975362258123091],"translation_xyz_m":[0.06708932969248735,0.10208084470736602,1.0192965771902482],"reprojection_error_px":0.23938415553837028}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.003718738809740408,0.32603298785188295,-0.21637434165458108,0.9202558373015548],"translation_xyz_m":[0.09968083215801239,0.08489385948233222,0.9896966482538602],"reprojection_error_px":0.14188874945009386},{"quaternion_xyzw":[-0.09620374440730989,-0.249705798818537,-0.18714253135349954,0.9451822716053311],"translation_xyz_m":[0.0997334957761834,0.08486995614100701,0.9896681314097563],"reprojection_error_px":0.2219870496065043}]}]},{"stamp_ns":1789379969238619886,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.056316650968766}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026010921898048805,-0.3405457962144029,-0.14442370888123157,0.9287054349661691],"translation_xyz_m":[0.06712799877917762,0.10206399942728818,1.0192319598073605],"reprojection_error_px":0.17836110869755895},{"quaternion_xyzw":[-0.12756054534864633,0.3844781147414712,-0.1755415219804234,0.8972681096617647],"translation_xyz_m":[0.06706487643320572,0.10206044787008428,1.0192205508888683],"reprojection_error_px":0.23333450904673728}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.004937058653614248,0.32553110837646115,-0.21801884457695653,0.920039622157857],"translation_xyz_m":[0.09964824487034538,0.08476636737294048,0.9898276877903542],"reprojection_error_px":0.14509763130558767},{"quaternion_xyzw":[-0.09500600341882072,-0.24938498871748477,-0.18898517854268254,0.945021475421566],"translation_xyz_m":[0.09970104856709412,0.08474273941782705,0.9898007290533749],"reprojection_error_px":0.2199881371140986}]}]},{"stamp_ns":1789379969271475759,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09076870013685122}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025910950245218313,-0.3404498245050609,-0.14459236076635848,0.928717173772443],"translation_xyz_m":[0.06712738180379334,0.1020871684221123,1.0193839742991568],"reprojection_error_px":0.17345793718872804},{"quaternion_xyzw":[-0.12748079061188686,0.3843562453668276,-0.17570939087338183,0.8972987989690995],"translation_xyz_m":[0.06706421281684166,0.10208355242177759,1.019371699617584],"reprojection_error_px":0.2308717369584995}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.005239924580881824,0.3257071267255447,-0.21828988674112443,0.9199113740666128],"translation_xyz_m":[0.09963153423073991,0.08475261738534803,0.9898244738779352],"reprojection_error_px":0.14386349304889556},{"quaternion_xyzw":[-0.09470649410729788,-0.24960871999358605,-0.18927385937413646,0.9449346924706954],"translation_xyz_m":[0.09968423442052797,0.08472891363646916,0.9897961609246914],"reprojection_error_px":0.2229752949442519}]}]},{"stamp_ns":1789379969304794717,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03739636058142215}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02601006697658561,-0.34099784854181364,-0.1449828797296647,0.9284524265093077],"translation_xyz_m":[0.06708625386757959,0.10201228432808805,1.0190457577342615],"reprojection_error_px":0.1502722052911595},{"quaternion_xyzw":[-0.12755092390515835,0.3848330082871206,-0.1761369615836999,0.8970006066373021],"translation_xyz_m":[0.06702326927554512,0.10200902689309845,1.019036732127874],"reprojection_error_px":0.20099639851372353}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.005566967143582228,0.325118045691719,-0.22053781050864307,0.9195816110484558],"translation_xyz_m":[0.09956880887685406,0.0845505823981919,0.989692647820236],"reprojection_error_px":0.14009650232547552},{"quaternion_xyzw":[-0.09439163136852591,-0.24925681825679433,-0.191651846555057,0.9445797098139069],"translation_xyz_m":[0.0996216116005482,0.08452696011272769,0.9896660875112475],"reprojection_error_px":0.2156836215362507}]}]},{"stamp_ns":1789379969338115500,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08020155567876298}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026359481294248265,-0.3412198733545221,-0.1452480931149684,0.9283195393937842],"translation_xyz_m":[0.06707998273346609,0.1019615495674389,1.0190160278201026],"reprojection_error_px":0.14327595947648283},{"quaternion_xyzw":[-0.12786338848600548,0.3850223937040612,-0.17638622963761083,0.8968258516710476],"translation_xyz_m":[0.06701708102963504,0.10195839715863246,1.0190083353295791],"reprojection_error_px":0.1901423281704941}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.005690806907985313,0.3246916874765669,-0.22130567323157596,0.9195470198939664],"translation_xyz_m":[0.0995838378642735,0.08447104767188904,0.99000174276885],"reprojection_error_px":0.14293167990009403},{"quaternion_xyzw":[-0.09424314432318569,-0.24889614476241576,-0.1924977420979499,0.94451763252873],"translation_xyz_m":[0.09963664445830421,0.08444747847481522,0.9899758979669592],"reprojection_error_px":0.2156076927752617}]}]},{"stamp_ns":1789379969371757888,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11886685801570746}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026668281788351934,-0.3410459924141515,-0.14634524111793265,0.928202297027344],"translation_xyz_m":[0.06708179739843932,0.10192896129407973,1.0191463325634489],"reprojection_error_px":0.14321512241808063},{"quaternion_xyzw":[-0.1281870101321571,0.38473613771130827,-0.1774234814552912,0.8966978883663368],"translation_xyz_m":[0.06701877727460749,0.10192562758453366,1.019136696167526],"reprojection_error_px":0.19587162843073713}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.006600780572399465,0.3237289809163665,-0.22418861084470712,0.9191819424783139],"translation_xyz_m":[0.09953974911763551,0.0842411294323296,0.9900683324174975],"reprojection_error_px":0.13631603978181173},{"quaternion_xyzw":[-0.0933693136044542,-0.2482226131619348,-0.19560992293369409,0.9441421840178352],"translation_xyz_m":[0.0995924588902378,0.08421757793822747,0.9900425515768037],"reprojection_error_px":0.21113920877339734}]}]},{"stamp_ns":1789379969405183786,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07877342386146925}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02707876100027989,-0.340779907079504,-0.1477817764427313,0.9280605272206144],"translation_xyz_m":[0.06706832210287042,0.10189399909468978,1.0191684356026942],"reprojection_error_px":0.15880991597232194},{"quaternion_xyzw":[-0.1286313774100669,0.38431435969529243,-0.1787828031404525,0.8965451193212912],"translation_xyz_m":[0.06700527723561871,0.10189063387994778,1.019158354036239],"reprojection_error_px":0.2119472548009636}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.008779088114001313,0.3232354422559958,-0.226829962106828,0.9186892536500426],"translation_xyz_m":[0.09952228186484584,0.08404681086347313,0.9905700674642487],"reprojection_error_px":0.15837473531068788},{"quaternion_xyzw":[-0.09120621793830844,-0.24801721589916229,-0.19856709319356475,0.9437902287731509],"translation_xyz_m":[0.09957551431268545,0.08402397639235044,0.9905494496077676],"reprojection_error_px":0.21245946234794802}]}]},{"stamp_ns":1789379969438294108,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.032836943407139225}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.027241764827628075,-0.34086629793823253,-0.1479507250564928,0.927997109981609],"translation_xyz_m":[0.0670409449043921,0.10187568775235858,1.0189919390739615],"reprojection_error_px":0.1536781881359569},{"quaternion_xyzw":[-0.12879786776404314,0.384365081853854,-0.17894978870687495,0.8964661545382965],"translation_xyz_m":[0.06697792831821321,0.10187237010094023,1.0189824660189681],"reprojection_error_px":0.20549025996776815}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.008683819717902578,0.32308808134808614,-0.2274714400251721,0.9185833805043471],"translation_xyz_m":[0.09948081958000589,0.08399036039747743,0.9904113677882819],"reprojection_error_px":0.14888369422840855},{"quaternion_xyzw":[-0.0913089065253462,-0.24795248614479104,-0.19922150726819482,0.9436593873032533],"translation_xyz_m":[0.09953375236618836,0.08396723267426463,0.9903879908773745],"reprojection_error_px":0.21336197418164277}]}]},{"stamp_ns":1789379969471632816,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.034677564249884746}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02767314284678427,-0.340958505169013,-0.14903838272906236,0.9277764037694638],"translation_xyz_m":[0.06701644130521842,0.1018483260956352,1.0190260207277047],"reprojection_error_px":0.16174815299367296},{"quaternion_xyzw":[-0.12924100669063868,0.3843162519342039,-0.18000101137212904,0.8962128188069207],"translation_xyz_m":[0.06695348998329716,0.10184510957166532,1.019017528103109],"reprojection_error_px":0.210560671396616}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.010079035912871265,0.3223384009796588,-0.23019234858603307,0.9181545898928949],"translation_xyz_m":[0.09942513507915125,0.08378562949712846,0.9905830196564045],"reprojection_error_px":0.16038500444072845},{"quaternion_xyzw":[-0.08994594139522086,-0.24750090922995574,-0.2021961596028941,0.9432760680727109],"translation_xyz_m":[0.09947855210493463,0.08376307160872883,0.9905649903071201],"reprojection_error_px":0.20677117786444574}]}]},{"stamp_ns":1789379969505031821,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06841291342406684}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.028431033667960313,-0.3410611509731364,-0.1501741734459024,0.9275325790781102],"translation_xyz_m":[0.06699137983618586,0.10177956346618705,1.0190776248706608],"reprojection_error_px":0.15842005281535854},{"quaternion_xyzw":[-0.12997255396980306,0.3842802609657804,-0.18106674582449703,0.8959077239331991],"translation_xyz_m":[0.06692804387100436,0.10177571512228926,1.0190632458577595],"reprojection_error_px":0.22241274561861069}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.011194066562930902,0.3211128988786929,-0.2332179379848115,0.9178074920426503],"translation_xyz_m":[0.09939499182237213,0.08354142376664933,0.9910625032362155],"reprojection_error_px":0.1427058277977022},{"quaternion_xyzw":[-0.0888296299738676,-0.24658932731833688,-0.20550888148121352,0.9429046081782274],"translation_xyz_m":[0.09944811589114089,0.08351880047270982,0.9910429726608384],"reprojection_error_px":0.19757353580446246}]}]},{"stamp_ns":1789379969538166889,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10048686187206585}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02855626031501672,-0.3409380689345806,-0.15070584963208725,0.9274877465684659],"translation_xyz_m":[0.06701640328627743,0.10178089330067494,1.0193990148743284],"reprojection_error_px":0.1622761181955228},{"quaternion_xyzw":[-0.13009925999393743,0.38411105986493943,-0.1815641686673378,0.8958612218946646],"translation_xyz_m":[0.0669531881197281,0.10177720710372258,1.019386159456951],"reprojection_error_px":0.22205753577124468}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.011399214235608865,0.3207641034600718,-0.23449268678422155,0.9176021075013855],"translation_xyz_m":[0.09938120811735711,0.08342255743039624,0.9911379526679156],"reprojection_error_px":0.15330466091436826},{"quaternion_xyzw":[-0.08861999734787852,-0.2463622635930636,-0.2068639938366156,0.9426873390479626],"translation_xyz_m":[0.09943448931152321,0.08340008040496213,0.9911205045737534],"reprojection_error_px":0.19958725369285585}]}]},{"stamp_ns":1789379969572017599,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12488378621703104}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.028673496901723758,-0.34109377242134803,-0.1518486567976705,0.927240451242041],"translation_xyz_m":[0.06700772238429076,0.10174712031165127,1.0193430805349355],"reprojection_error_px":0.15315282959629414},{"quaternion_xyzw":[-0.1302446188237908,0.38413607012475515,-0.1826996770410727,0.895598485318975],"translation_xyz_m":[0.06694461900692923,0.10174367521013314,1.0193320362509222],"reprojection_error_px":0.2090966488834827}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01319601312669963,0.31989848573331586,-0.23741316264129697,0.9171291153745739],"translation_xyz_m":[0.09933628163263018,0.08320151198546948,0.9912691182207392],"reprojection_error_px":0.14216438238477874},{"quaternion_xyzw":[-0.08686280546201107,-0.24580118842035115,-0.21009005489430518,0.942283820105662],"translation_xyz_m":[0.09938919268005324,0.08317894636971834,0.9912484089842121],"reprojection_error_px":0.201270815971343}]}]},{"stamp_ns":1789379969605249755,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06878736563503869}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.029195655815397337,-0.3412033051055477,-0.15277829167182994,0.9270311277731708],"translation_xyz_m":[0.06697919184078843,0.10170439294767485,1.0193235299742178],"reprojection_error_px":0.15643907190397133},{"quaternion_xyzw":[-0.13076219083076424,0.38412216210977895,-0.18358762406438067,0.895347417662911],"translation_xyz_m":[0.06691619816009722,0.10170109799153736,1.0193141929246397],"reprojection_error_px":0.20763573419060807}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.013894407097023874,0.31896371196485995,-0.24004528078274615,0.9167591608895723],"translation_xyz_m":[0.09929041270410462,0.08300123193682651,0.991541829322089],"reprojection_error_px":0.13994429854624882},{"quaternion_xyzw":[-0.08617960909892308,-0.24514982600962604,-0.21292343500269345,0.9418801667991851],"translation_xyz_m":[0.09934332423845636,0.08297876307824698,0.9915223034493115],"reprojection_error_px":0.19607213338519466}]}]},{"stamp_ns":1789379969638492411,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06112930832364667}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0297709871906923,-0.341496413316574,-0.15400937466328188,0.9267011387332121],"translation_xyz_m":[0.06691149569578629,0.1016340911084529,1.0189912292039474],"reprojection_error_px":0.15063985335026248},{"quaternion_xyzw":[-0.1313437708448836,0.3842364772250514,-0.18479245026438365,0.8949653031013071],"translation_xyz_m":[0.06684842778222269,0.10163071711414201,1.0189811287958093],"reprojection_error_px":0.20418082978657429}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.015505945070213396,0.31804510697135246,-0.24360171476084136,0.9161141196186413],"translation_xyz_m":[0.09922490060360493,0.08275816442590399,0.9919848365819456],"reprojection_error_px":0.14309660479312333},{"quaternion_xyzw":[-0.08460027725065279,-0.24463777884302018,-0.21678136138026707,0.9412762567971128],"translation_xyz_m":[0.09927803041015931,0.0827360705063321,0.9919683372295178],"reprojection_error_px":0.18906445719433862}]}]},{"stamp_ns":1789379969671696715,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.044257537852080385}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.030266209854048338,-0.3413076268614118,-0.1551964470862051,0.9265565947092066],"translation_xyz_m":[0.06688749058121053,0.10159158466388962,1.0190403447235816],"reprojection_error_px":0.15671801858239462},{"quaternion_xyzw":[-0.13184994787708235,0.38390708460309786,-0.18590556098638666,0.8948016897786332],"translation_xyz_m":[0.06682446537179065,0.1015882548642913,1.0190308138568134],"reprojection_error_px":0.20839135756056432}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01668673732963215,0.31746688727684974,-0.24580474907472263,0.915705385815958],"translation_xyz_m":[0.09918559959036581,0.08257850074428984,0.9923074321351949],"reprojection_error_px":0.1497811994283717},{"quaternion_xyzw":[-0.08341334910637084,-0.24430781725350326,-0.21919925908926133,0.9409078533174259],"translation_xyz_m":[0.09923857750171933,0.08255643261750474,0.9922898564454954],"reprojection_error_px":0.19822552988168193}]}]},{"stamp_ns":1789379969704934070,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07477214434086127}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.030302031334168936,-0.3413122378333282,-0.1562335969527885,0.9263794073625679],"translation_xyz_m":[0.06688652105248075,0.10154343397778631,1.0192651993850341],"reprojection_error_px":0.14442127597179832},{"quaternion_xyzw":[-0.13187529097510367,0.38379723649718867,-0.1869135567965488,0.8946350715084984],"translation_xyz_m":[0.0668233959269084,0.10153999871297516,1.0192539648997447],"reprojection_error_px":0.20137449073233}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01785004030198371,0.3166522348200247,-0.2484998644831618,0.9152379775755329],"translation_xyz_m":[0.09914288422288121,0.08233588386117109,0.9925709730708263],"reprojection_error_px":0.15001087737953464},{"quaternion_xyzw":[-0.08222919982263707,-0.24377184022567452,-0.2221442567649877,0.9404603010206483],"translation_xyz_m":[0.09919617629205635,0.08231422859988882,0.9925573069956146],"reprojection_error_px":0.18593994553463386}]}]},{"stamp_ns":1789379969738467834,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08837215617044679}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03080720785089167,-0.34146040339380496,-0.15753220327920225,0.9260881781928608],"translation_xyz_m":[0.06687276187680734,0.10148978717429402,1.0192367943583047],"reprojection_error_px":0.147239315284614},{"quaternion_xyzw":[-0.1323845497857919,0.3837961884643009,-0.1881688892648012,0.8942970903514889],"translation_xyz_m":[0.06680964960765527,0.10148638659138734,1.0192258212447387],"reprojection_error_px":0.20326005477742248}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.018973329856162912,0.31563965478458567,-0.2517202830765431,0.9146849294535806],"translation_xyz_m":[0.09907529753614346,0.0820695128741984,0.9926288303032768],"reprojection_error_px":0.14117093625023447},{"quaternion_xyzw":[-0.08112385392256062,-0.24309531690943767,-0.22562349763028136,0.939902986769661],"translation_xyz_m":[0.09912816924332923,0.08204763670240228,0.9926119502084735],"reprojection_error_px":0.18948906864324874}]}]},{"stamp_ns":1789379969771721717,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08423641046185973}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03153871212184885,-0.34158138586259545,-0.1587996415631069,0.9258024304943859],"translation_xyz_m":[0.06685084245504894,0.10143883019754978,1.0191959586419086],"reprojection_error_px":0.1540657025957292},{"quaternion_xyzw":[-0.1331177916260491,0.3837646056875049,-0.1893768433284237,0.8939467501954907],"translation_xyz_m":[0.06678775636860801,0.1014354433351483,1.0191854431666938],"reprojection_error_px":0.20839160585163727}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.020748913706911523,0.314899157295407,-0.25480370171114525,0.9140476338292898],"translation_xyz_m":[0.09903448477106427,0.08183590616520572,0.9930663809972997],"reprojection_error_px":0.15358383861857142},{"quaternion_xyzw":[-0.07935277440924884,-0.24269022083995578,-0.22900743483529623,0.9393402944047304],"translation_xyz_m":[0.09908778187335887,0.0818146056372201,0.9930542252247372],"reprojection_error_px":0.18464375290200163}]}]},{"stamp_ns":1789379969804938317,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06169101361484453}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.032156991162247815,-0.341678674120509,-0.1600338902414477,0.9255326928557577],"translation_xyz_m":[0.06681447429233182,0.10139305744480029,1.0190898964179262],"reprojection_error_px":0.14847188108863735},{"quaternion_xyzw":[-0.13375163337325147,0.38370301067310925,-0.19056256035156138,0.8936265499427805],"translation_xyz_m":[0.06675129484036292,0.10138952277967736,1.0190780746291934],"reprojection_error_px":0.2066489804671725}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.021775472798657988,0.31358269397972327,-0.2580166332119181,0.9135749229301341],"translation_xyz_m":[0.0989879867939732,0.08162061890284396,0.9935126984323205],"reprojection_error_px":0.13728795578669528},{"quaternion_xyzw":[-0.07835869584733998,-0.24172175080083463,-0.23248799933294625,0.9388183211575322],"translation_xyz_m":[0.09904078610981865,0.08159908114778279,0.9934971505458553],"reprojection_error_px":0.18229997718828453}]}]},{"stamp_ns":1789379969838369418,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04888235465159477}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.032753769429390886,-0.34163986405212804,-0.1621949156653978,0.9251498274392663],"translation_xyz_m":[0.06677625448257621,0.10133121510360546,1.019197689705435],"reprojection_error_px":0.15892876276028375},{"quaternion_xyzw":[-0.13437137473772431,0.38340082748125115,-0.19264345019919543,0.8932170174336895],"translation_xyz_m":[0.06671309309424564,0.10132773969868204,1.0191860495698155],"reprojection_error_px":0.21568222270326665}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.024022388630830078,0.31232577038813497,-0.26275410421600276,0.9125983885113832],"translation_xyz_m":[0.09889405442474461,0.08126165242076738,0.9940465214742985],"reprojection_error_px":0.15063143915152752},{"quaternion_xyzw":[-0.07611915399320739,-0.2409945613714452,-0.23764379811175665,0.9378981399937638],"translation_xyz_m":[0.09894730031670466,0.08124076193465841,0.9940365012013855],"reprojection_error_px":0.17519871917358315}]}]},{"stamp_ns":1789379969871653714,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06292275754414342}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03331957222443256,-0.3417054581646325,-0.16381767721289786,0.924819417291749],"translation_xyz_m":[0.0667311851904284,0.10124930643365197,1.0192170574085315],"reprojection_error_px":0.16602793827461856},{"quaternion_xyzw":[-0.1349216012683724,0.3832606369419912,-0.1941991804875359,0.8928572808573423],"translation_xyz_m":[0.06666798080505888,0.10124578426138055,1.0192047609586536],"reprojection_error_px":0.22369173256314304}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.025406847933483866,0.3108863787963944,-0.26679166504800655,0.9118806714794769],"translation_xyz_m":[0.09880374305972495,0.08093020042648987,0.9944608668722835],"reprojection_error_px":0.14244980453739242},{"quaternion_xyzw":[-0.07472014822730941,-0.23999904168603065,-0.24202248388841946,0.9371459207248121],"translation_xyz_m":[0.09885667762138124,0.08090925909910894,0.9944493280043917],"reprojection_error_px":0.17338566782270226}]}]},{"stamp_ns":1789379969904961459,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08140017131738679}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03368611059300341,-0.3420105415286541,-0.1649845748944923,0.9244858708943455],"translation_xyz_m":[0.06669398071445665,0.10117486203083972,1.0191057203331422],"reprojection_error_px":0.15496007384494007},{"quaternion_xyzw":[-0.13527282143425629,0.38341213649426886,-0.19534739804798748,0.8924885385516146],"translation_xyz_m":[0.06663086649990478,0.10117152587011574,1.019094979885664],"reprojection_error_px":0.20962670498038688}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.02729640300687718,0.31002139514144383,-0.2701049982258159,0.9111448462625857],"translation_xyz_m":[0.09873126445639556,0.08066132245534359,0.9947453386001489],"reprojection_error_px":0.15333213505502127},{"quaternion_xyzw":[-0.0728257974395214,-0.23950005842564218,-0.24565995789990946,0.9364760062735331],"translation_xyz_m":[0.09878447598567235,0.08064084427657951,0.9947370427997936],"reprojection_error_px":0.17226737807047393}]}]},{"stamp_ns":1789379969938500471,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06524024859082475}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.033894324034405406,-0.34203068517973406,-0.16560995409909646,0.9243589823748279],"translation_xyz_m":[0.0667005214095742,0.101153684728373,1.0190485587108993],"reprojection_error_px":0.1608572368657323},{"quaternion_xyzw":[-0.13549421596007935,0.3833734422345108,-0.19595119794542654,0.8923392007831875],"translation_xyz_m":[0.06663758770561176,0.10115063600113787,1.0190406429949193],"reprojection_error_px":0.2078243442140226}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.07200335057087982,-0.23931154005322228,-0.247354189359666,0.9361417677414808],"translation_xyz_m":[0.0987796832720721,0.08053601759748462,0.9949538165205142],"reprojection_error_px":0.159357073551785},{"quaternion_xyzw":[-0.02813086975150324,0.3096776487687356,-0.2716613449435412,0.9107735842038668],"translation_xyz_m":[0.09872588065704663,0.08055591235440358,0.9949559256134298],"reprojection_error_px":0.16317363423290734}]}]},{"stamp_ns":1789379969971650504,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08280314129488492}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03444470657014004,-0.3420014521524323,-0.16754363874619568,0.9240009188472751],"translation_xyz_m":[0.06667932518042291,0.10109453341655705,1.019099546455448],"reprojection_error_px":0.15301359916455692},{"quaternion_xyzw":[-0.13607090179785078,0.38312425853670784,-0.19781362093851532,0.8919474668251549],"translation_xyz_m":[0.06661596291412292,0.10109086560597419,1.0190850454181808],"reprojection_error_px":0.21733848842976689}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.029996520164911153,0.30787684333461324,-0.27587067842353336,0.9100590238563923],"translation_xyz_m":[0.09865986045082012,0.08024404879673704,0.9954790164299717],"reprojection_error_px":0.1391191870165988},{"quaternion_xyzw":[-0.070153777416141,-0.2379571405625412,-0.2519742563128746,0.9353950079646135],"translation_xyz_m":[0.09871283393201438,0.08022379645541172,0.9954703766497518],"reprojection_error_px":0.1599805656443553}]}]},{"stamp_ns":1789379970005315544,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06056954833561809}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03510186800082827,-0.34191900161180583,-0.16903720057118543,0.9237346372323033],"translation_xyz_m":[0.06664755762106653,0.1010504995164793,1.0192081406469942],"reprojection_error_px":0.14762578580767918},{"quaternion_xyzw":[-0.13673994275375784,0.3828567935263921,-0.19923048701966742,0.8916445910497606],"translation_xyz_m":[0.06658387739566714,0.10104632424600817,1.019188658899612],"reprojection_error_px":0.22478687700038558}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03180468379729097,0.3068741864815394,-0.2797146620051954,0.9091624737191736],"translation_xyz_m":[0.09856582070236304,0.07995148107308586,0.9958309604447165],"reprojection_error_px":0.14970834144565787},{"quaternion_xyzw":[-0.06834537652799122,-0.23739265002178195,-0.25614829059379063,0.9345382241774608],"translation_xyz_m":[0.09861901745493043,0.07993163100240619,0.9958253364054952],"reprojection_error_px":0.15927055129827145}]}]},{"stamp_ns":1789379970038548287,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03504967173042342}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03575792752416902,-0.34182712114262254,-0.17075173751200398,0.9234280881623403],"translation_xyz_m":[0.06661935346340138,0.10101263181309601,1.0194522239247956],"reprojection_error_px":0.16006913499409833},{"quaternion_xyzw":[-0.1374048452896176,0.3825530446859338,-0.20086194189329698,0.8913066569881053],"translation_xyz_m":[0.06655538758407172,0.10100800006047427,1.0194281158505984],"reprojection_error_px":0.245000346425152}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03273309336572787,0.3050526059341478,-0.28372120087213987,0.9085008158428737],"translation_xyz_m":[0.09850477672552803,0.07968569640603733,0.9965478025005728],"reprojection_error_px":0.13735575851997014},{"quaternion_xyzw":[-0.06743364414428944,-0.23600852857078333,-0.26046417957888496,0.9337617946968989],"translation_xyz_m":[0.0985575716846712,0.07966573201255407,0.9965406479638713],"reprojection_error_px":0.15283289406180728}]}]},{"stamp_ns":1789379970072088338,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05301295175051715}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03654357614171845,-0.342125902825411,-0.17233427558080802,0.9229925953758837],"translation_xyz_m":[0.0665637054388705,0.10090376107427973,1.0192284054436185],"reprojection_error_px":0.15581989328217027},{"quaternion_xyzw":[-0.13816448207629192,0.3826453659862586,-0.20238789233067173,0.8908042662783898],"translation_xyz_m":[0.06650009328890533,0.10089968425052251,1.0192099827933774],"reprojection_error_px":0.22888202369587288}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03514752583016425,0.30404738450715163,-0.28785959313011494,0.9074451465765537],"translation_xyz_m":[0.09839383881216714,0.07932485172541075,0.9968233916665526],"reprojection_error_px":0.14007330773470786},{"quaternion_xyzw":[-0.06497746149700168,-0.2354675671477661,-0.26500901539563615,0.9327932118524198],"translation_xyz_m":[0.09844683182785652,0.07930534245312203,0.9968185873354944],"reprojection_error_px":0.14606718894712298}]}]},{"stamp_ns":1789379970105086682,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07024656434965029}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03662129416862021,-0.34240274109409236,-0.17303291671507554,0.9227561180711515],"translation_xyz_m":[0.06653538904485018,0.10083431765853801,1.019006385875371],"reprojection_error_px":0.14935238326150357},{"quaternion_xyzw":[-0.1382222139582241,0.38283331751995,-0.203090577162887,0.8905545957619899],"translation_xyz_m":[0.06647181872557109,0.10083037932744954,1.0189888775625286],"reprojection_error_px":0.22130165911113792}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03631460644101669,0.3028510357271072,-0.29065095261298063,0.906909324719522],"translation_xyz_m":[0.09834541381037662,0.07911374111850954,0.9971394956444924],"reprojection_error_px":0.1434905050835983},{"quaternion_xyzw":[-0.0638177218842747,-0.23456278329020883,-0.2680619153700478,0.9322287319090702],"translation_xyz_m":[0.09839835923713383,0.07909439922736873,0.9971354561212722],"reprojection_error_px":0.14685214410686823}]}]},{"stamp_ns":1789379970138683141,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05854016702548164}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03665913168135258,-0.342432945014687,-0.17344462540954791,0.9226661087031859],"translation_xyz_m":[0.06653408467551833,0.10083620473443813,1.0190986408557845],"reprojection_error_px":0.1455781008243982},{"quaternion_xyzw":[-0.1382708614685951,0.38281272782866804,-0.2034982179348776,0.8904628344734191],"translation_xyz_m":[0.06647039383894496,0.10083209805014602,1.0190792102410773],"reprojection_error_px":0.22275851775516362}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.036586367946716214,0.3025775689972901,-0.29168387937198614,0.9066580209399057],"translation_xyz_m":[0.09831545471707515,0.0790360500181419,0.9971908655467683],"reprojection_error_px":0.14142256393161062},{"quaternion_xyzw":[-0.06354601549341368,-0.23440802957714685,-0.26916075805691037,0.931969562757765],"translation_xyz_m":[0.09836828641203194,0.07901664355591062,0.9971859950798292],"reprojection_error_px":0.1482121629736797}]}]},{"stamp_ns":1789379970171812249,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04415703168148341}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03699232922869917,-0.342545959352052,-0.17391471144080425,0.9225223609508224],"translation_xyz_m":[0.06652388863353143,0.10082931065073772,1.019042217778727],"reprojection_error_px":0.14808159254060704},{"quaternion_xyzw":[-0.13861494789454737,0.38286410397153603,-0.203954138139199,0.890282923371083],"translation_xyz_m":[0.0664601980710329,0.10082519181175259,1.0190228582871907],"reprojection_error_px":0.22461292810960323}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03695548538044017,0.3021503854491578,-0.29259014316852305,0.9064934885556788],"translation_xyz_m":[0.09831865518847495,0.07899897217188839,0.9974947538159161],"reprojection_error_px":0.13520968401991018},{"quaternion_xyzw":[-0.06318952706955148,-0.23408019970704277,-0.27015005367279726,0.9317899399942148],"translation_xyz_m":[0.09837142827990727,0.07897960078427088,0.9974898554657003],"reprojection_error_px":0.1414125363483755}]}]},{"stamp_ns":1789379970205179907,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.052517000324237555}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03799336569696825,-0.3421972384429715,-0.17642020622345253,0.9221352747840715],"translation_xyz_m":[0.06650879449346266,0.10078789411723181,1.0194437938937713],"reprojection_error_px":0.1719593913227817},{"quaternion_xyzw":[-0.13964596298859766,0.3822288523469641,-0.20631666487459033,0.8898502926051866],"translation_xyz_m":[0.06644503712865359,0.10078361516200056,1.019422967236604],"reprojection_error_px":0.2472811184566121}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.06085539234915096,-0.23288522550295884,-0.27584859177823967,0.9305743642387905],"translation_xyz_m":[0.09825692698402705,0.07858603938941339,0.9980556944649742],"reprojection_error_px":0.13113422992580293},{"quaternion_xyzw":[-0.03928154344070294,0.3003778588994138,-0.29782439013078726,0.9052793684112903],"translation_xyz_m":[0.09820397313974594,0.07860491974957569,0.9980571210422958],"reprojection_error_px":0.13995034406261211}]}]},{"stamp_ns":1789379970238380635,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.035948353815921125}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03900702680194783,-0.34253845088714086,-0.1790592206578277,0.9214573549662085],"translation_xyz_m":[0.06641015744374251,0.10061860541261848,1.0189427877822188],"reprojection_error_px":0.1566650865439152},{"quaternion_xyzw":[-0.1406457961561591,0.38223144866289793,-0.20887297876236902,0.8890950221543238],"translation_xyz_m":[0.06634644361904245,0.10061447232023325,1.0189231749165149],"reprojection_error_px":0.2319580951377622}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.057861390797817366,-0.23144618224651425,-0.2829838079505444,0.9289805641757339],"translation_xyz_m":[0.09809717720632306,0.07806490352456923,0.9988200747553847],"reprojection_error_px":0.12292819843393175},{"quaternion_xyzw":[-0.0422244262174334,0.2982055199974016,-0.3043720784769053,0.9036858987044871],"translation_xyz_m":[0.09804409451100443,0.07808324195313533,0.9988182169951942],"reprojection_error_px":0.1457903085540803}]}]},{"stamp_ns":1789379970271945966,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06407321438531483}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03999400162061928,-0.34238509252846255,-0.18187982221097995,0.9209194636455179],"translation_xyz_m":[0.06636145542776194,0.10052108904961883,1.019156369130846],"reprojection_error_px":0.16771483641691456},{"quaternion_xyzw":[-0.1416365213932135,0.3817389362957308,-0.21155998305376797,0.8885138456398545],"translation_xyz_m":[0.06629775494909329,0.10051697383983392,1.0191367407996974],"reprojection_error_px":0.2409174043588959}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.05452939561384939,-0.22985465820267767,-0.29046645913476093,0.9272662062397361],"translation_xyz_m":[0.09790460262184925,0.07752175746927117,0.9993132145358105],"reprojection_error_px":0.12408961309198069},{"quaternion_xyzw":[-0.04552764386326169,0.2958464105833207,-0.31122428538591657,0.9019653979922129],"translation_xyz_m":[0.0978514964859392,0.07753959118527261,0.9993092690531405],"reprojection_error_px":0.1536361827260342}]}]},{"stamp_ns":1789379970305274403,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06387050204017893}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04119789836208332,-0.34274292467194517,-0.18441493449009302,0.9202288588687508],"translation_xyz_m":[0.06629964521177645,0.10039954795158963,1.0189660784210939],"reprojection_error_px":0.16284244945845078},{"quaternion_xyzw":[-0.1428308436846983,0.3817804517605023,-0.2140057878471466,0.8877187389673522],"translation_xyz_m":[0.06623591593275599,0.10039540348307711,1.0189462523363317],"reprojection_error_px":0.23716493206361894}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.05137911049036235,-0.22793092371590315,-0.2974718728058204,0.9256987446835132],"translation_xyz_m":[0.09777407015840958,0.07706803693643084,1.0002864953501414],"reprojection_error_px":0.11452350395889449},{"quaternion_xyzw":[-0.048654856459550117,0.29320998548655913,-0.31760799943937246,0.9004364319850636],"translation_xyz_m":[0.0977215982921502,0.07708581625846808,1.0002865417226048],"reprojection_error_px":0.13193329496250014}]}]},{"stamp_ns":1789379970338624380,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.039258827867762504}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04193569802049551,-0.34272271612435457,-0.18678485631791966,0.9197249341700882],"translation_xyz_m":[0.06626963084709966,0.1003367595176138,1.0192081627271221],"reprojection_error_px":0.17113705953831168},{"quaternion_xyzw":[-0.14358068312641453,0.3814744494187384,-0.21628442348093682,0.8871769158590306],"translation_xyz_m":[0.06620606360951653,0.1003328751123724,1.019190610448938],"reprojection_error_px":0.23898100875269354}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.048976999688256966,-0.2265035131131187,-0.3035473697053071,0.924205824691633],"translation_xyz_m":[0.09763826782288601,0.07665147537372645,1.000963272658767],"reprojection_error_px":0.11653284979024116},{"quaternion_xyzw":[-0.05102587529978305,0.29115536595729913,-0.323197506887813,0.8989818043017692],"translation_xyz_m":[0.09758613629552942,0.0766691299511543,1.0009648014138837],"reprojection_error_px":0.12698769781487004}]}]},{"stamp_ns":1789379970371735111,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05409987476546167}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.042450776035565,-0.3427291736423164,-0.18856599730959026,0.919335363078715],"translation_xyz_m":[0.06625910471190262,0.10031494888239038,1.0195734197999413],"reprojection_error_px":0.16775868626138804},{"quaternion_xyzw":[-0.1441127085054306,0.3812656868044848,-0.21800450958517553,0.8867592892737354],"translation_xyz_m":[0.06619534438307854,0.10031077384959204,1.019552564353493],"reprojection_error_px":0.2430429045556567}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.04717342345670031,-0.22488008328902703,-0.30843642568190516,0.9230766964731942],"translation_xyz_m":[0.09756661644440025,0.07636611878206148,1.0018608424232398],"reprojection_error_px":0.11124070258842919},{"quaternion_xyzw":[-0.05283216960849361,0.28903896024692954,-0.32767864980269684,0.8979376057262078],"translation_xyz_m":[0.09751459276404124,0.07638346761314795,1.0018610086754192],"reprojection_error_px":0.12849710111453705}]}]},{"stamp_ns":1789379970471927562,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08826172972994477}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04490461197989304,-0.3426848219841778,-0.19544585309448925,0.9177971492180473],"translation_xyz_m":[0.06609758929599698,0.10003881593568574,1.0195270559276366],"reprojection_error_px":0.1740093796720362},{"quaternion_xyzw":[-0.14657362241603142,0.38037472934180694,-0.22459803797146125,0.8850914979985741],"translation_xyz_m":[0.06603376475566043,0.10003460625518976,1.019505266844734],"reprojection_error_px":0.24957782940040507}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03888707462928074,-0.2214109127500722,-0.3252926833309362,0.9185040409891052],"translation_xyz_m":[0.09707167359162668,0.07514099895855558,1.0030857307322334],"reprojection_error_px":0.1001388308563805},{"quaternion_xyzw":[-0.0609516657766009,0.28379135710249415,-0.3430852074464912,0.8933307900796773],"translation_xyz_m":[0.0970196787985477,0.0751571372495175,1.0030828453096838],"reprojection_error_px":0.13276146279575954}]}]},{"stamp_ns":1789379970505218729,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08442587170838649}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.045608289006017294,-0.34284357143947186,-0.19698784126834215,0.9173734026496569],"translation_xyz_m":[0.06607071346521401,0.09998812449479869,1.0196083140240602],"reprojection_error_px":0.18645497817137988},{"quaternion_xyzw":[-0.14726938178200102,0.3803399927313823,-0.22608209801246779,0.8846129685217825],"translation_xyz_m":[0.06600702669704477,0.09998411570478982,1.019588595475462],"reprojection_error_px":0.2556459900114797}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.036899345967301586,-0.22064001062160638,-0.3293338890932686,0.9173307001702198],"translation_xyz_m":[0.09697100328329723,0.07486896443045798,1.0035527532952822],"reprojection_error_px":0.08904054900036389},{"quaternion_xyzw":[-0.06289969841469113,0.28259275857246824,-0.3467880430423743,0.8921451753748288],"translation_xyz_m":[0.0969187876428125,0.07488463512554434,1.0035467904638231],"reprojection_error_px":0.13845556791859345}]}]},{"stamp_ns":1789379970538678759,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.052684263171707886}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04611898150571604,-0.34276880021448897,-0.19810755479605682,0.9171346607108046],"translation_xyz_m":[0.06608042721201093,0.09999047344385134,1.0199071480060886],"reprojection_error_px":0.18463029932422848},{"quaternion_xyzw":[-0.14780039426693953,0.38013956020733985,-0.22714628774239046,0.8843379004570361],"translation_xyz_m":[0.06601669144814021,0.09998634916853782,1.0198863843438104],"reprojection_error_px":0.25602776577987063}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.035333272019606794,-0.2198983854821741,-0.33254236867749926,0.9164124797191927],"translation_xyz_m":[0.09690147691807795,0.07466435488360174,1.0038895394261143],"reprojection_error_px":0.10048323248100466},{"quaternion_xyzw":[-0.06445584695772985,0.28151925476030865,-0.3497240153686097,0.8912269441991858],"translation_xyz_m":[0.09684969971918121,0.07468011026206886,1.003887347719638],"reprojection_error_px":0.12991962949272806}]}]},{"stamp_ns":1789379970571894534,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07018104952977186}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04709559608270667,-0.34333144459161435,-0.20011408554240256,0.9164386923039521],"translation_xyz_m":[0.06598615299257157,0.09982875825963584,1.0191802040451534],"reprojection_error_px":0.15988642308546713},{"quaternion_xyzw":[-0.14875168515398612,0.38043686101085095,-0.22910843663365965,0.8835440312804678],"translation_xyz_m":[0.06592242966235376,0.09982473533478436,1.0191604213502907],"reprojection_error_px":0.23342420206225567}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03230381864739127,-0.2182670025289426,-0.33906076925758327,0.9145237961137385],"translation_xyz_m":[0.09672840483816605,0.07422922807237203,1.004541945018183],"reprojection_error_px":0.07967874050310092},{"quaternion_xyzw":[-0.06743311854711587,0.2792063694801193,-0.35569169900053477,0.8893705600180696],"translation_xyz_m":[0.09667619267376247,0.07424413962377553,1.0045334194940314],"reprojection_error_px":0.14310817945539664}]}]},{"stamp_ns":1789379970605252239,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.038296693055772116}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04756561543205229,-0.3433665774233654,-0.20234681770716614,0.9159108423317834],"translation_xyz_m":[0.06594111685175642,0.09976140606388546,1.019471019733771],"reprojection_error_px":0.15588458996102647},{"quaternion_xyzw":[-0.14921439565200692,0.38019109670622153,-0.23127254812308815,0.883007815707302],"translation_xyz_m":[0.06587681993523004,0.09975656604015605,1.0194421075967115],"reprojection_error_px":0.24921310246257233}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.029015356298294454,-0.21706008206092328,-0.3449023217329953,0.9127351304391531],"translation_xyz_m":[0.09654389402467768,0.07381305376692998,1.0050579163885713],"reprojection_error_px":0.08987777705373548},{"quaternion_xyzw":[-0.07063813217936545,0.2773647048296927,-0.3610035782887111,0.8875559088078309],"translation_xyz_m":[0.09649223912125397,0.07382789502783835,1.0050543567205743],"reprojection_error_px":0.1291465370414323}]}]},{"stamp_ns":1789379970638625816,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04882369896618832}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.047855066524717535,-0.3432531617577238,-0.20320379641276987,0.9157485335367338],"translation_xyz_m":[0.0659333244366391,0.09975460097736163,1.0196642255326025],"reprojection_error_px":0.1678244537762103},{"quaternion_xyzw":[-0.14951125244584168,0.3799731021441047,-0.232086194973446,0.8828379382093915],"translation_xyz_m":[0.06586913044070195,0.09974990108926438,1.019636699598055],"reprojection_error_px":0.2553786509903143}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.029452886909948083,-0.2155668429135946,-0.3465875689352166,0.9124365845079382],"translation_xyz_m":[0.09653630732013542,0.07373916395888877,1.0057059538660256],"reprojection_error_px":0.08173073362716976},{"quaternion_xyzw":[-0.0702051010427895,0.27571488000087485,-0.3625861022834227,0.8874592200008912],"translation_xyz_m":[0.09648481756116736,0.0737539803836505,1.0057010471554133],"reprojection_error_px":0.13106689532372345}]}]},{"stamp_ns":1789379970671813026,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07959368737444608}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04856395517627317,-0.34342325899754383,-0.20489156163860145,0.9152712469024895],"translation_xyz_m":[0.06586034100694527,0.09963920598623958,1.019202729234829],"reprojection_error_px":0.16949695624683822},{"quaternion_xyzw":[-0.15020764671979228,0.3799246905015595,-0.23371453992016242,0.8823108331218104],"translation_xyz_m":[0.06579643514330652,0.09963499494657614,1.0191801257425237],"reprojection_error_px":0.24678227551404344}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.02727020460230756,-0.21480363091957183,-0.3511018419659524,0.9109573165921838],"translation_xyz_m":[0.09637774373571317,0.07341002004347061,1.0059252041116995],"reprojection_error_px":0.07050308607431201},{"quaternion_xyzw":[-0.07232775355658841,0.27446004844757793,-0.36674181914510307,0.8859688572188928],"translation_xyz_m":[0.09632587565330956,0.07342425152113517,1.0059157716728888],"reprojection_error_px":0.14248701904123934}]}]},{"stamp_ns":1789379970705288071,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07631811841543615}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04993271802889845,-0.3434871715014727,-0.20765558922399188,0.9145504048155033],"translation_xyz_m":[0.06580327140394163,0.09952683651576649,1.0191662099328727],"reprojection_error_px":0.1749462428072874},{"quaternion_xyzw":[-0.15156660039946138,0.37965360910028095,-0.23634685032743316,0.8814935445485594],"translation_xyz_m":[0.06573917472108762,0.09952230907931855,1.0191406876867304],"reprojection_error_px":0.2566580002964906}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.02356631789966366,-0.21294384811705594,-0.3578906018064666,0.9088530482692773],"translation_xyz_m":[0.09619026035981057,0.07294496808492448,1.006708534805138],"reprojection_error_px":0.07627735638172069},{"quaternion_xyzw":[-0.07594126609080534,0.2718857359395376,-0.3729020825940047,0.8838863657134868],"translation_xyz_m":[0.09613884664491187,0.07295889751377561,1.0067021598341899],"reprojection_error_px":0.13330081387504925}]}]},{"stamp_ns":1789379970738617650,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.057759771539094125}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.050511935911041146,-0.3437203669269159,-0.20993872074662556,0.9139095071293524],"translation_xyz_m":[0.06576007625164002,0.09946004793333338,1.0192914341673254],"reprojection_error_px":0.16563357894063957},{"quaternion_xyzw":[-0.1521506218531661,0.37959478340335184,-0.23858014750714993,0.8808163837589102],"translation_xyz_m":[0.06569633195688789,0.09945611893933518,1.019271310408209],"reprojection_error_px":0.23800347188595586}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.020170873186664207,-0.21149246593230295,-0.364716006299636,0.9065573933720936],"translation_xyz_m":[0.09595779922734879,0.07248333950183078,1.007075237566419],"reprojection_error_px":0.08490483237004956},{"quaternion_xyzw":[-0.07926526960560257,0.2696860549524199,-0.37916780061839567,0.8815998115770898],"translation_xyz_m":[0.09590687803408086,0.07249712510773945,1.007072883589916],"reprojection_error_px":0.12164391378037716}]}]},{"stamp_ns":1789379970772019106,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05224137008496096}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05192491456337313,-0.3434229106977708,-0.21313821077141354,0.9132012980524161],"translation_xyz_m":[0.06569802940672871,0.09935747608846501,1.019248274046827],"reprojection_error_px":0.2013514629240638},{"quaternion_xyzw":[-0.1535811947604973,0.3789208627998136,-0.24160769260449277,0.8800326807711836],"translation_xyz_m":[0.06563386418421395,0.0993528778243068,1.0192215947060546],"reprojection_error_px":0.27945083092640605}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.016529870555514234,-0.20955961153246178,-0.37186369656459717,0.9041730607417447],"translation_xyz_m":[0.09574459308402576,0.07202360928789477,1.0078604777303786],"reprojection_error_px":0.07335407946081043},{"quaternion_xyzw":[-0.08282504581796066,0.26698248216249165,-0.38569514122297666,0.8792608395923224],"translation_xyz_m":[0.09569351043990258,0.07203662965492724,1.0078545730492146],"reprojection_error_px":0.13026495628905474}]}]},{"stamp_ns":1789379970805348481,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.041922820734585454}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05284899420622783,-0.3438149956523847,-0.21528421663453703,0.912497089663301],"translation_xyz_m":[0.06561486788038925,0.09922174926327242,1.0189266475298473],"reprojection_error_px":0.18029302493926316},{"quaternion_xyzw":[-0.1544707807707317,0.3790279903169646,-0.24369177272715195,0.8792558673955275],"translation_xyz_m":[0.06555073223927692,0.09921725075831704,1.0189007965252828],"reprojection_error_px":0.2607644402345475}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.013132331903117257,-0.207163452038229,-0.378319292707494,0.902100525863771],"translation_xyz_m":[0.09555618662764742,0.07159784141817242,1.0087487133318316],"reprojection_error_px":0.08260744627621575},{"quaternion_xyzw":[-0.08612718462682747,0.26390298839398724,-0.391534862056782,0.8772900162313236],"translation_xyz_m":[0.0955057283277803,0.07161061237831552,1.0087468450741008],"reprojection_error_px":0.1187538030922311}]}]},{"stamp_ns":1789379970838682482,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05620320771020032}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.053405949825145765,-0.3435698958922214,-0.2172596974659424,0.9120886771673515],"translation_xyz_m":[0.0655984337574307,0.09919025053484538,1.0193079964393752],"reprojection_error_px":0.19574535301322943},{"quaternion_xyzw":[-0.1550381601850476,0.3785493666225641,-0.24557457466330634,0.8788382525783838],"translation_xyz_m":[0.0655343722490382,0.09918585550885287,1.0192829019339849],"reprojection_error_px":0.2715774713617864}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.010630821785248752,-0.2065532473419416,-0.3830970507464339,0.9002551812403267],"translation_xyz_m":[0.09536387295261434,0.07125814018752065,1.0088638360612388],"reprojection_error_px":0.0663679040291141},{"quaternion_xyzw":[-0.08854955548363014,0.26275304975487895,-0.39594496174517146,0.8754127017223227],"translation_xyz_m":[0.09531286732992073,0.07127019976458232,1.008856182811801],"reprojection_error_px":0.13434699802571065}]}]},{"stamp_ns":1789379970871932096,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.055539862303437285}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05436874251150883,-0.34377843663980245,-0.21979625833003605,0.911345176737841],"translation_xyz_m":[0.06554495006570553,0.09911460594310517,1.0195609285050629],"reprojection_error_px":0.1941376055751714},{"quaternion_xyzw":[-0.1559833350010585,0.37843043131604454,-0.24802608736176862,0.8780334092982525],"translation_xyz_m":[0.06548062555268612,0.09910982132535938,1.019531523893817],"reprojection_error_px":0.27759585012513793}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.007318059632228202,-0.20437519654478806,-0.3897078671467127,0.8979448776648034],"translation_xyz_m":[0.09516375833220143,0.07083362452888244,1.0098677637326572],"reprojection_error_px":0.09571274767189145},{"quaternion_xyzw":[-0.09175126366888378,0.2598476054811763,-0.401969903683989,0.8732016514374824],"translation_xyz_m":[0.09511403846443305,0.07084597138754285,1.0098713598259426],"reprojection_error_px":0.10272244684793473}]}]},{"stamp_ns":1789379970905637934,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07240019001008298}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05541263946485723,-0.34381202771534014,-0.2222800323901654,0.9106669622789831],"translation_xyz_m":[0.06546320807022468,0.0990009570720389,1.0195222926345449],"reprojection_error_px":0.17831859531538696},{"quaternion_xyzw":[-0.1570056686157382,0.37814017925713783,-0.2504024998641203,0.8773014378853508],"translation_xyz_m":[0.06539877175580648,0.09899601053185693,1.0194911855415931],"reprojection_error_px":0.2680149866935262}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.002894608303508569,-0.20354512205256495,-0.39672214431092434,0.8950824234363658],"translation_xyz_m":[0.0948160339920279,0.07029343813558733,1.0096648582467442],"reprojection_error_px":0.07837814172543346},{"quaternion_xyzw":[-0.09601354058979975,0.25820520726810825,-0.4083843233248798,0.8702492260413244],"translation_xyz_m":[0.09476555706364491,0.07030470487297047,1.0096614034873217],"reprojection_error_px":0.12417045330988509}]}]},{"stamp_ns":1789379970972112480,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07032303201630122}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05717874387434322,-0.34370544277914594,-0.2277144002795778,0.9092542613363193],"translation_xyz_m":[0.06538149816187312,0.09884182975590332,1.0196243253713777],"reprojection_error_px":0.20229004073998036},{"quaternion_xyzw":[-0.15879799860624325,0.37738774453002377,-0.2556293845629388,0.8757940988983461],"translation_xyz_m":[0.06531725549831285,0.09883724212885925,1.019596132936774],"reprojection_error_px":0.28103564832416095}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.0037885903975189788,-0.1996115509030199,-0.4097371505673362,0.8900878286853781],"translation_xyz_m":[0.09440053640385,0.069476496018168,1.011104393010397],"reprojection_error_px":0.09219423546972803},{"quaternion_xyzw":[-0.10250342511141687,0.2528471518033766,-0.42028501727904405,0.865402721232203],"translation_xyz_m":[0.09435078447890277,0.06948704272455093,1.011105144929361],"reprojection_error_px":0.11550721463467362}]}]},{"stamp_ns":1789379971005781944,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09250273672892412}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.057873509174996014,-0.3438406930233989,-0.23005370623367793,0.9085700451836992],"translation_xyz_m":[0.06533611085831047,0.09875631636665543,1.0197842786699056],"reprojection_error_px":0.19132919944320176},{"quaternion_xyzw":[-0.1594746666931626,0.3772310484878798,-0.25789558457386375,0.875073959272669],"translation_xyz_m":[0.06527190344382576,0.09875182662692775,1.0197565156016932],"reprojection_error_px":0.27117753030749236}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.00748555931936727,-0.19800965053575365,-0.41624663522917676,0.8873978157268438],"translation_xyz_m":[0.0941297593113488,0.0690117557402692,1.011423231230488],"reprojection_error_px":0.08602742139113533},{"quaternion_xyzw":[-0.10605312016231275,0.25051813859604816,-0.42623144904916127,0.8627399085353948],"translation_xyz_m":[0.09407966949513936,0.06902146482668821,1.0114196600364684],"reprojection_error_px":0.13027835748743158}]}]},{"stamp_ns":1789379971038781848,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05185088220832608}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05838526122945486,-0.34403992592437094,-0.23177117019130372,0.9080252283440562],"translation_xyz_m":[0.06527849789862727,0.09870728999242911,1.01998187722571],"reprojection_error_px":0.1806280380140364},{"quaternion_xyzw":[-0.15996857996950226,0.3771861674708477,-0.2595731488035034,0.8745069633292972],"translation_xyz_m":[0.06521427582470184,0.09870280856446045,1.0199537169114112],"reprojection_error_px":0.26320307188700875}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.010095428373737727,-0.19632774046967108,-0.42120669278995587,0.8854029718704202],"translation_xyz_m":[0.09392809609558257,0.06870276816356193,1.011916544728782],"reprojection_error_px":0.10303359543212211},{"quaternion_xyzw":[-0.1085718177300874,0.24826040665685706,-0.4307597014130152,0.8608280958007392],"translation_xyz_m":[0.09387876952377783,0.06871251541975186,1.0119197032175382],"reprojection_error_px":0.11376325453062573}]}]},{"stamp_ns":1789379971072062459,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10988066776751802}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.059797434466600734,-0.3440298732532531,-0.23448229414994684,0.9072407436125987],"translation_xyz_m":[0.06517474661549429,0.09857710955735138,1.019663159385033],"reprojection_error_px":0.18825105839805337},{"quaternion_xyzw":[-0.1613618962978738,0.37682010383008513,-0.2621503246988695,0.8736396024865832],"translation_xyz_m":[0.06511061848863918,0.09857275540949606,1.0196367483846456],"reprojection_error_px":0.26570683137526163}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.014105480072621732,-0.19495998977487955,-0.4275509327951128,0.8826051425665549],"translation_xyz_m":[0.0936272257072402,0.06826093529917286,1.0122302168222537],"reprojection_error_px":0.10167885726035146},{"quaternion_xyzw":[-0.11241833231029749,0.24614045772691187,-0.4365425593605534,0.8580254002635911],"translation_xyz_m":[0.09357780700267448,0.06827001928454872,1.0122323956463548],"reprojection_error_px":0.11715426635139482}]}]},{"stamp_ns":1789379971105428417,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09198417297841964}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06088724306139847,-0.34394090747400297,-0.23720056502615036,0.9064950566603308],"translation_xyz_m":[0.06512592159622148,0.098493164799366,1.019738985425747],"reprojection_error_px":0.19586742076706848},{"quaternion_xyzw":[-0.162452478811306,0.3764014205794705,-0.26474803726144125,0.8728342565917273],"translation_xyz_m":[0.06506191104024252,0.09848898541594685,1.0197143321449045],"reprojection_error_px":0.26833595419904965}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.01767269588091767,-0.19335012753190234,-0.4339215220100402,0.8797815164801928],"translation_xyz_m":[0.09335320900719804,0.06782874980482263,1.0125423970216063],"reprojection_error_px":0.10851210162164102},{"quaternion_xyzw":[-0.1158413014390043,0.2438023269500458,-0.44238002381481417,0.8552433178834344],"translation_xyz_m":[0.09330378100751131,0.0678372528384249,1.01254382013301],"reprojection_error_px":0.12645398819417708}]}]},{"stamp_ns":1789379971138813813,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03515066563621806}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06150355701437449,-0.34394705547968674,-0.23970532736598665,0.9057919692367548],"translation_xyz_m":[0.06509236119723062,0.0984130486932074,1.0198173984911911],"reprojection_error_px":0.20352057394285714},{"quaternion_xyzw":[-0.16306945130656797,0.3761146041149448,-0.26717259219462586,0.8721037579336746],"translation_xyz_m":[0.06502819934456036,0.09840870628889568,1.0197903114946207],"reprojection_error_px":0.27851354331337147}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.021327152467989274,-0.1913109171635728,-0.440593287200867,0.8768254334901412],"translation_xyz_m":[0.09310896577388073,0.06741968982144964,1.0131721071059443],"reprojection_error_px":0.11505591700694166},{"quaternion_xyzw":[-0.11937303449465694,0.24100999757147484,-0.4484853499338388,0.8523644470535225],"translation_xyz_m":[0.09305981729036977,0.06742772511049297,1.0131752054109664],"reprojection_error_px":0.1249780389998992}]}]},{"stamp_ns":1789379971172014640,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06490078007840223}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06223142515152625,-0.34401936276434253,-0.24174520150538856,0.905172461642501],"translation_xyz_m":[0.0650494235304629,0.09833688858264207,1.019911215208882],"reprojection_error_px":0.18717613685410023},{"quaternion_xyzw":[-0.1637821374140209,0.37593487885089044,-0.26913815356990867,0.8714430748020443],"translation_xyz_m":[0.06498483513689111,0.0983319223596332,1.0198773676435027],"reprojection_error_px":0.27690231735914095}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.025384652558691878,-0.19020326155853962,-0.4463353929099745,0.8740498016376795],"translation_xyz_m":[0.09284090530605983,0.06701471842299415,1.0133806036532753],"reprojection_error_px":0.12019356581414502},{"quaternion_xyzw":[-0.12326580766191365,0.23922642765052224,-0.4537000221374167,0.8495484370461364],"translation_xyz_m":[0.09279177843961625,0.06702221035477071,1.0133847199194863],"reprojection_error_px":0.12535535810841195}]}]},{"stamp_ns":1789379971238779052,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07120143933607224}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06414558599262236,-0.3443626673421421,-0.24643692415353363,0.9036418203870457],"translation_xyz_m":[0.06490253355807703,0.09817161193948952,1.0199381318677605],"reprojection_error_px":0.17519707668431336},{"quaternion_xyzw":[-0.1656585339866588,0.3756486651358852,-0.2736713353882801,0.8697984425634563],"translation_xyz_m":[0.06483788551422508,0.09816659137895609,1.0199033265624533],"reprojection_error_px":0.26890278833938236}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.12992309690453077,0.23507647929113562,-0.4646003527428955,0.8438042130767048],"translation_xyz_m":[0.09219936443955258,0.0662318807887494,1.013847630409116],"reprojection_error_px":0.11706398400214262},{"quaternion_xyzw":[0.03235735683349915,-0.18749217519125483,-0.45820326223507984,0.8682450438545168],"translation_xyz_m":[0.09224811045605806,0.0662251507475482,1.0138394804366517],"reprojection_error_px":0.1297393128040587}]}]},{"stamp_ns":1789379971272146363,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0915302956027727}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06549860376316677,-0.3440072964086311,-0.24965799481494483,0.9027955463712042],"translation_xyz_m":[0.06483585991287484,0.09808452404933857,1.0201133976372876],"reprojection_error_px":0.224508626100688},{"quaternion_xyzw":[-0.16700446472357972,0.3748954077613116,-0.2767221699337078,0.8689003295367373],"translation_xyz_m":[0.06477183196087952,0.09808040189160117,1.020088096812207],"reprojection_error_px":0.2914145305433676}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.13404535907270457,0.23190676835897142,-0.4708809433962353,0.8405487669651531],"translation_xyz_m":[0.09186895159263166,0.06577981822275433,1.0143164599737615],"reprojection_error_px":0.13010180299815122},{"quaternion_xyzw":[0.03667734033643303,-0.18514055078329808,-0.46511489555232766,0.8648964580207563],"translation_xyz_m":[0.09191714418365379,0.06577348650951739,1.0143031965472291],"reprojection_error_px":0.16048936618547943}]}]},{"stamp_ns":1789379971305384021,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05184789097975619}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06677347719902445,-0.3444508989756565,-0.2522461745243459,0.901813034046108],"translation_xyz_m":[0.06475084671198857,0.09794445001267675,1.0196746431953279],"reprojection_error_px":0.21539467231094866},{"quaternion_xyzw":[-0.1682452666304424,0.37500494061941503,-0.27923053745383225,0.8678105390698618],"translation_xyz_m":[0.06468683469273227,0.09794040008531299,1.0196500595906137],"reprojection_error_px":0.2822250503007975}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.13813717741912987,0.22917341008280687,-0.47739714567016833,0.8369525874454714],"translation_xyz_m":[0.09155507047987305,0.0653348406414686,1.0148385260324813],"reprojection_error_px":0.12180307946883656},{"quaternion_xyzw":[0.04095412118079151,-0.18325440135080148,-0.4722228584346261,0.8612468614257468],"translation_xyz_m":[0.09160272257586936,0.06532880847956898,1.0148195735369618],"reprojection_error_px":0.17280778095033308}]}]},{"stamp_ns":1789379971338711819,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06687942648087693}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0676709766224539,-0.34400663407609866,-0.2551238209475107,0.9011059375121989],"translation_xyz_m":[0.06473863531434872,0.09790051249609183,1.020204525114494],"reprojection_error_px":0.23667409275433252},{"quaternion_xyzw":[-0.16915289890124757,0.3742340590657749,-0.28196294405840017,0.8670830779149008],"translation_xyz_m":[0.06467459033489746,0.09789638596621475,1.0201788662480773],"reprojection_error_px":0.30141255496944425}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.14231999096401385,0.22719802647487658,-0.48341853864222173,0.8333262226972957],"translation_xyz_m":[0.09121494917767846,0.06488876455721729,1.0148089272047771],"reprojection_error_px":0.12030623512069939},{"quaternion_xyzw":[0.045327213517224976,-0.1820763011495068,-0.47878824455784474,0.8576453119720854],"translation_xyz_m":[0.09126275953130514,0.06488339304736306,1.014790422389733],"reprojection_error_px":0.17052103033564087}]}]},{"stamp_ns":1789379971372267632,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08081923586123033}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0680948406974842,-0.34442237029448264,-0.25732989979683635,0.9002875352806262],"translation_xyz_m":[0.06465991605735935,0.0977856262882209,1.0200853473412934],"reprojection_error_px":0.19866457015671982},{"quaternion_xyzw":[-0.1695392355194523,0.3743567259389139,-0.28414408102749594,0.8662422470537997],"translation_xyz_m":[0.06459514491657517,0.0977805300386016,1.020048415012938],"reprojection_error_px":0.28825366023023596}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1462815893851257,0.22432852258181682,-0.4899159194423256,0.8296148518681434],"translation_xyz_m":[0.09085718641261073,0.06443063083736213,1.015193911840784],"reprojection_error_px":0.10196651976710829},{"quaternion_xyzw":[0.04950183113367054,-0.18007157095354967,-0.4858624457003299,0.8538509717197893],"translation_xyz_m":[0.09090452135972885,0.06442560101770645,1.015170439968846],"reprojection_error_px":0.17408408447880871}]}]},{"stamp_ns":1789379971405415592,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04245097685749393}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06976972553811778,-0.3441872896261296,-0.26067592847061527,0.899286025341291],"translation_xyz_m":[0.06457434640422507,0.09769086839443113,1.020108989438077],"reprojection_error_px":0.23909109009905077},{"quaternion_xyzw":[-0.17120356973899956,0.3736956032341393,-0.28731484549411035,0.8651538090945399],"translation_xyz_m":[0.06450994061625392,0.09768626550700507,1.0200777194875448],"reprojection_error_px":0.3110287093413868}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1502419540772301,0.22188202375002847,-0.4960263468550088,0.8259258961900406],"translation_xyz_m":[0.0904750479648016,0.0639953569760537,1.0153037851049862],"reprojection_error_px":0.1152560363480702},{"quaternion_xyzw":[0.05368138809757377,-0.17846379258541684,-0.49251270452936813,0.850117767832854],"translation_xyz_m":[0.09052247543363594,0.06399099264312968,1.015280813142957],"reprojection_error_px":0.1813268048849452}]}]},{"stamp_ns":1789379971438785830,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10078978299466702}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07054880196548424,-0.34438348277043473,-0.2630543481218565,0.898457173865469],"translation_xyz_m":[0.06446565825035885,0.09758320492366641,1.0198854159306208],"reprojection_error_px":0.21919856029156698},{"quaternion_xyzw":[-0.17195613433929188,0.3735536697860062,-0.28963110814411885,0.8642931012380606],"translation_xyz_m":[0.06440154477273557,0.09757911959585425,1.0198590054006682],"reprojection_error_px":0.2866731865211727}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15401608401608935,0.21983538274309652,-0.5020282224559341,0.8221430010748009],"translation_xyz_m":[0.09009313458928832,0.06357813397048837,1.0154669914103949],"reprojection_error_px":0.11722586037406836},{"quaternion_xyzw":[0.05768346769048362,-0.17726149349378573,-0.4990041774290701,0.846313069370839],"translation_xyz_m":[0.09014024464557388,0.06357403899121095,1.0154394174294323],"reprojection_error_px":0.1961862734049448}]}]},{"stamp_ns":1789379971472096065,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11387378563967294}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07150758770477339,-0.34428025753312935,-0.2655626190729438,0.8976827192971273],"translation_xyz_m":[0.06440645737818528,0.09751086649540823,1.0200862972579128],"reprojection_error_px":0.2207436553353703},{"quaternion_xyzw":[-0.17289596307050809,0.3731299792878252,-0.29203105362261544,0.8634806704441355],"translation_xyz_m":[0.06434237616058319,0.09750682516705712,1.020060186957428],"reprojection_error_px":0.2870220858729201}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15732937661923194,0.21682854270259402,-0.507701922283996,0.8188233072105414],"translation_xyz_m":[0.08978299149731128,0.06320654558359105,1.0160666132853313],"reprojection_error_px":0.11950869233215203},{"quaternion_xyzw":[0.06119655925600314,-0.17501613193661814,-0.505183408214501,0.8428606401784056],"translation_xyz_m":[0.08982980692223581,0.063202916352275,1.016036807670491],"reprojection_error_px":0.20390934218202542}]}]},{"stamp_ns":1789379971505613751,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04030071252323397}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0725357137112005,-0.3446002074027129,-0.26818084390048347,0.8966985570743595],"translation_xyz_m":[0.06434341532784338,0.09739685036124117,1.0198270120110495],"reprojection_error_px":0.22365030964066154},{"quaternion_xyzw":[-0.1738994043790237,0.373123767473076,-0.2945747318864319,0.8624171720445711],"translation_xyz_m":[0.06427944023239629,0.09739303558931903,1.0198028595429147],"reprojection_error_px":0.2859173368828614}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1615130520526286,0.2154094930180323,-0.5135185765541651,0.8147459455978551],"translation_xyz_m":[0.08939878118601058,0.06276711061823416,1.015781662845123],"reprojection_error_px":0.11216751568463336},{"quaternion_xyzw":[0.06561527640587252,-0.1744074883355573,-0.5114822232177287,0.838845992329291],"translation_xyz_m":[0.0894458583310766,0.06276406046941528,1.0157520593178053],"reprojection_error_px":0.19908495005266538}]}]},{"stamp_ns":1789379971538804069,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09231817782739604}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07355707820503087,-0.3443291075954613,-0.2708265158121342,0.8959240036082927],"translation_xyz_m":[0.06430842061562791,0.0973175771371548,1.019877801234846],"reprojection_error_px":0.24937607067019016},{"quaternion_xyzw":[-0.17491838260762507,0.37254917138347143,-0.29708891565705847,0.8615966866934519],"translation_xyz_m":[0.06424449075399973,0.09731382647736561,1.0198542803085986],"reprojection_error_px":0.30653162275057405}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16486497777391346,0.21254930579906095,-0.5189445590549071,0.8113808454328044],"translation_xyz_m":[0.08910713698451662,0.06240439387763059,1.0161175240041738],"reprojection_error_px":0.11696938898538663},{"quaternion_xyzw":[0.06915336708546554,-0.17226104055214728,-0.5174003349477613,0.835368684548672],"translation_xyz_m":[0.08915384566123306,0.06240172994223925,1.0160843327905824],"reprojection_error_px":0.211729227315163}]}]},{"stamp_ns":1789379971572146314,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09369185993882188}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07430849349866565,-0.3441950275028656,-0.27309128408485317,0.8952257711846473],"translation_xyz_m":[0.06425713772738446,0.09725260753975383,1.0200787550995367],"reprojection_error_px":0.24371280688371022},{"quaternion_xyzw":[-0.1756540568625081,0.3721299995391239,-0.2992594354198637,0.8608767078176401],"translation_xyz_m":[0.06419299366581778,0.09724854280529717,1.0200516317750938],"reprojection_error_px":0.3066220472153855}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16851703524055509,0.21104332156465394,-0.5241306537190626,0.8076817337842424],"translation_xyz_m":[0.0887539892157281,0.06202971170563504,1.0160129536473939],"reprojection_error_px":0.11123824038339966},{"quaternion_xyzw":[0.07302667987330347,-0.17149352276606178,-0.5230133079121241,0.8316935465799805],"translation_xyz_m":[0.08880109666419223,0.06202770842248712,1.0159822157145395],"reprojection_error_px":0.20192351855232363}]}]},{"stamp_ns":1789379971605601040,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05149108687834523}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07536509296780664,-0.344148035112001,-0.2757228142666732,0.8943484569133937],"translation_xyz_m":[0.06418175049358477,0.09719583738486146,1.020353256190192],"reprojection_error_px":0.23441949073170795},{"quaternion_xyzw":[-0.17669316337164506,0.37172403935617904,-0.30178751166787054,0.8599564305148034],"translation_xyz_m":[0.06411782900777739,0.09719209574676316,1.0203293945869365],"reprojection_error_px":0.2932233437176905}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17198180696438134,0.20866294704466803,-0.5294345070761107,0.8041026895372106],"translation_xyz_m":[0.08839280717140252,0.06167496227205564,1.016295980267413],"reprojection_error_px":0.11049343097321394},{"quaternion_xyzw":[0.07672729613290949,-0.16988185051800955,-0.5287724202836053,0.8280415487402354],"translation_xyz_m":[0.08843948200145933,0.061673199898212336,1.0162597987227588],"reprojection_error_px":0.21605274781597553}]}]},{"stamp_ns":1789379971639026413,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13070401282157548}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0761021428806769,-0.3442988624159877,-0.27775744668164576,0.8935980964622287],"translation_xyz_m":[0.06408451908949717,0.09712022202703087,1.0203685084188316],"reprojection_error_px":0.23492713099349524},{"quaternion_xyzw":[-0.17739040810023554,0.3715759362356202,-0.3037624711597645,0.8591811961626996],"translation_xyz_m":[0.06402046260460491,0.09711630924412905,1.0203424704388036],"reprojection_error_px":0.29665625027210807}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17541102038791193,0.20711621452302667,-0.5349129482769879,0.8001262308996898],"translation_xyz_m":[0.08795866704083061,0.06128422609990777,1.0159433402937967],"reprojection_error_px":0.12247788274715266},{"quaternion_xyzw":[0.08041068018904726,-0.1691477318379052,-0.5346465961517316,0.8240607893529421],"translation_xyz_m":[0.08800551572376009,0.06128289971703202,1.015906969147457],"reprojection_error_px":0.22351265861612904}]}]},{"stamp_ns":1789379971672389391,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13125606995133896}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07729778468123327,-0.3437705602032716,-0.28082750119875155,0.8927389142360536],"translation_xyz_m":[0.0640514452044244,0.09708968977645874,1.020992313204536],"reprojection_error_px":0.25218726704379013},{"quaternion_xyzw":[-0.17858895865475652,0.37067400939753414,-0.3066662767229852,0.858290485397938],"translation_xyz_m":[0.06398761314788921,0.0970860432087248,1.0209691901334845],"reprojection_error_px":0.3064222972221894}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17914694727287708,0.2043624466334853,-0.5406001750699843,0.7961744861543149],"translation_xyz_m":[0.08759248171928957,0.06091651583054216,1.0162209643727975],"reprojection_error_px":0.11388424568246834},{"quaternion_xyzw":[0.08438316661948429,-0.16720284389759715,-0.5408268826000926,0.8200176664199923],"translation_xyz_m":[0.08763908132321291,0.060915627912225545,1.0161813358736236],"reprojection_error_px":0.22682528176557257}]}]},{"stamp_ns":1789379971706042365,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.047698984472785144}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07809865673919883,-0.3440673561435774,-0.28316697428543364,0.8918154063066718],"translation_xyz_m":[0.0639987473788178,0.09697810046045612,1.0207177427984824],"reprojection_error_px":0.22760947055822936},{"quaternion_xyzw":[-0.17936750779439073,0.37068670294388595,-0.3089479058562741,0.857303829966508],"translation_xyz_m":[0.06393470519672202,0.09697422242338176,1.0206915885407282],"reprojection_error_px":0.289669390360044}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18303388960927935,0.20208801256629158,-0.5461879781976199,0.7920465408698536],"translation_xyz_m":[0.08724578344353436,0.06054757393588306,1.0163067078948844],"reprojection_error_px":0.11338713428277566},{"quaternion_xyzw":[0.08849784588485829,-0.16570869497594495,-0.5468918507893263,0.8158541923795783],"translation_xyz_m":[0.08729275191100078,0.06054746054230506,1.0162699742899597],"reprojection_error_px":0.21934206617617347}]}]},{"stamp_ns":1789379971739093516,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1147212277772586}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07923713604083053,-0.34411226897406505,-0.28576030987210366,0.890869950057452],"translation_xyz_m":[0.06394103527135987,0.09687543436666576,1.0204700594969298],"reprojection_error_px":0.23316585378201315},{"quaternion_xyzw":[-0.18049120231754456,0.37041695974025546,-0.31143739454241415,0.8562832189780353],"translation_xyz_m":[0.0638770488695909,0.09687166848706463,1.0204450338070776],"reprojection_error_px":0.29213038774226496}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18636093991059055,0.20105166868800373,-0.5516925124182641,0.7877075588916447],"translation_xyz_m":[0.08684228402515826,0.06016470100641629,1.015835892263888],"reprojection_error_px":0.10300164069380408},{"quaternion_xyzw":[0.09206598780393094,-0.16547273119331193,-0.5527430708448144,0.8115526641901663],"translation_xyz_m":[0.08688948230525316,0.06016490475446032,1.0157987327893359],"reprojection_error_px":0.21499430323705018}]}]},{"stamp_ns":1789379971772267515,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10866817669120929}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07993529325633156,-0.34413155042454646,-0.2879222217354635,0.8901037125668765],"translation_xyz_m":[0.06387164266433677,0.09679727091854987,1.0205044169157171],"reprojection_error_px":0.23920385219051882},{"quaternion_xyzw":[-0.18115749775781007,0.37015136673141913,-0.31352515932772784,0.8554951204895996],"translation_xyz_m":[0.06380752098753746,0.0967933387803594,1.0204771819347018],"reprojection_error_px":0.3002017945829383}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1902477917869171,0.19823801642185288,-0.5569492780959496,0.7837824750488693],"translation_xyz_m":[0.08647496626230522,0.05982579851113804,1.0160521642225222],"reprojection_error_px":0.11322862839954337},{"quaternion_xyzw":[0.09620383986474806,-0.1634286336507291,-0.5584941375890058,0.8075395972811775],"translation_xyz_m":[0.08652208017650667,0.059826588083591814,1.016013378633993],"reprojection_error_px":0.22450849847711424}]}]},{"stamp_ns":1789379971805757279,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.044789860584523916}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08122164708246297,-0.3435372286599584,-0.2910730420716873,0.8891915995717576],"translation_xyz_m":[0.06380057259902672,0.0967488173738467,1.0209986741322663],"reprojection_error_px":0.2817665128772642},{"quaternion_xyzw":[-0.18242667623958086,0.3691535173822391,-0.316497523171477,0.854561587146778],"translation_xyz_m":[0.06373671057347247,0.09674520164139584,1.0209750100136163],"reprojection_error_px":0.3312687871936305}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1939831232108587,0.19588367918223898,-0.5626219342475689,0.7793950803301972],"translation_xyz_m":[0.08604217669962176,0.059459157713291515,1.0161301603155835],"reprojection_error_px":0.1126030841100647},{"quaternion_xyzw":[0.10022942627064617,-0.161933395077903,-0.5646104487103583,0.8030857232419768],"translation_xyz_m":[0.08608881944222906,0.059460097670733916,1.0160843435808546],"reprojection_error_px":0.24117083562162214}]}]},{"stamp_ns":1789379971839015356,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12935408829432554}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0823220353488525,-0.3438687215453637,-0.29353305661910245,0.8881529876719585],"translation_xyz_m":[0.06367731895603208,0.09660546816016823,1.0204596612620944],"reprojection_error_px":0.2701885032034927},{"quaternion_xyzw":[-0.1834750393614252,0.36913857686951157,-0.3188867744535043,0.853454653790531],"translation_xyz_m":[0.06361367321039495,0.09660225616968464,1.0204400084102034],"reprojection_error_px":0.3149477022254625}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19731642934590216,0.19377740305489263,-0.5675319855557073,0.7755153061975513],"translation_xyz_m":[0.08565152385903885,0.05912627915979315,1.0161514351532874],"reprojection_error_px":0.11022013593550671},{"quaternion_xyzw":[0.10383289484268299,-0.1605701860835548,-0.5699149269738634,0.7991451190503868],"translation_xyz_m":[0.08569844087938021,0.05912783736202933,1.016107464252704],"reprojection_error_px":0.23558745804436215}]}]},{"stamp_ns":1789379971872453753,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.140363579898371}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08281378044899732,-0.3440076816104318,-0.2953210876065572,0.887460448682442],"translation_xyz_m":[0.06362359067250904,0.0965362431447122,1.0204477734701471],"reprojection_error_px":0.26338736751906106},{"quaternion_xyzw":[-0.18393890213439731,0.36904323297577946,-0.320630965715652,0.8527422566641255],"translation_xyz_m":[0.06355984156746873,0.09653293474906317,1.020426490220442],"reprojection_error_px":0.3109234572197591}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20030038365352673,0.19224858078403897,-0.572091476710068,0.7717717160990462],"translation_xyz_m":[0.08531858724361405,0.05883741711162133,1.0160494299585776],"reprojection_error_px":0.1080095723780874},{"quaternion_xyzw":[0.10703427427143879,-0.15971976214614783,-0.5748035323757714,0.7953830277793836],"translation_xyz_m":[0.08536578287676616,0.0588394462617132,1.0160068713521053],"reprojection_error_px":0.2310130753312516}]}]},{"stamp_ns":1789379971905678911,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07314907640886543}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08367615595707854,-0.34375385858748414,-0.29792714434804957,0.8866064528814795],"translation_xyz_m":[0.06360062464271778,0.09649059367752294,1.0206636943380367],"reprojection_error_px":0.2929417913329096},{"quaternion_xyzw":[-0.18480371140891286,0.368483981868501,-0.3231247594790529,0.8518553475605055],"translation_xyz_m":[0.0635372560790084,0.09648786772635413,1.0206482720125178],"reprojection_error_px":0.32895453032481314}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20299853328129203,0.1904746211712298,-0.5766378100883768,0.7681144772439861],"translation_xyz_m":[0.08500029862168312,0.05855816437640106,1.0161887506072163],"reprojection_error_px":0.1073384711025826},{"quaternion_xyzw":[0.10995704398473613,-0.15861589223252,-0.5796667774736314,0.7916671486822956],"translation_xyz_m":[0.08504739089677958,0.058560428879021166,1.0161438214626557],"reprojection_error_px":0.2363372134274245}]}]},{"stamp_ns":1789379971938952572,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09499699309189742}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08445773043273855,-0.34387591248912547,-0.3001704177977771,0.8857279316240569],"translation_xyz_m":[0.06354622467361577,0.09641111212960644,1.0205416134858092],"reprojection_error_px":0.2929387653988443},{"quaternion_xyzw":[-0.1855663721927928,0.36832380241689555,-0.3253008612762292,0.8509301074338061],"translation_xyz_m":[0.06348278163416096,0.0964083282980853,1.0205251246349194],"reprojection_error_px":0.3299806010383602}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20588614444946135,0.18928736001750052,-0.5817348211723726,0.7637838625533327],"translation_xyz_m":[0.08459567380459601,0.058228834635264834,1.0157012575771784],"reprojection_error_px":0.12992218806552275},{"quaternion_xyzw":[0.11309896432794607,-0.15819395853199353,-0.5850566464707734,0.7873322146161026],"translation_xyz_m":[0.08464235199760617,0.058230934624215386,1.0156485721376893],"reprojection_error_px":0.26515824004141897}]}]},{"stamp_ns":1789379971972440349,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.056671269356702046}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08540769212299,-0.3439830433894999,-0.3020819340636127,0.8849450249021737],"translation_xyz_m":[0.06350387778067945,0.09639496320923313,1.0210247942526547],"reprojection_error_px":0.2750600493742097},{"quaternion_xyzw":[-0.18648727561461242,0.36816365508464965,-0.327141154372796,0.850092162194303],"translation_xyz_m":[0.06344007436565849,0.09639158314907384,1.0210020455762348],"reprojection_error_px":0.32191070607012134}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20933889328718,0.18711321212191265,-0.5865628008881997,0.7596775330499838],"translation_xyz_m":[0.08419649907563569,0.057931580171179886,1.015400959153906],"reprojection_error_px":0.11449514471516445},{"quaternion_xyzw":[0.11679172531010809,-0.1567709692164483,-0.5902714798073373,0.7831743970764621],"translation_xyz_m":[0.0842437813136934,0.05793450949462524,1.0153537088808602],"reprojection_error_px":0.24523803633309085}]}]},{"stamp_ns":1789379972006065205,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06356301080344158}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0861728638345834,-0.34418601796345616,-0.30429822161847786,0.8840321345385798],"translation_xyz_m":[0.06338892714503326,0.09629181873481762,1.0207272677470345],"reprojection_error_px":0.2677460917792123},{"quaternion_xyzw":[-0.1872098348741447,0.368041810415842,-0.32930346221018164,0.8491507129412379],"translation_xyz_m":[0.06332549337002963,0.09628907891346423,1.0207107657551553],"reprojection_error_px":0.30614091598370075}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21225306269356276,0.18546167732580437,-0.591544907296682,0.7553987200624991],"translation_xyz_m":[0.08381561746625697,0.057655057261826015,1.0156793336271024],"reprojection_error_px":0.1015746884934766},{"quaternion_xyzw":[0.11997737473382503,-0.1559036323732697,-0.5955598693824268,0.7788503893213294],"translation_xyz_m":[0.08386288571403666,0.05765821339250927,1.0156300994415064],"reprojection_error_px":0.24366281608917806}]}]},{"stamp_ns":1789379972039249028,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13419063982243176}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0872729277505272,-0.34374583168223827,-0.30688736173926323,0.8832000829300529],"translation_xyz_m":[0.06331371546812734,0.09621638735070866,1.0208437685913183],"reprojection_error_px":0.3122889235691911},{"quaternion_xyzw":[-0.18827773370066955,0.3672736324582834,-0.3317458792302669,0.8482960836330523],"translation_xyz_m":[0.06325037147134645,0.09621374626186434,1.0208285208199124],"reprojection_error_px":0.34479029208769824}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21549623378915833,0.18332898021815164,-0.5962413897885431,0.7512975864049394],"translation_xyz_m":[0.08340749511608415,0.057350677626667006,1.0154651958333285],"reprojection_error_px":0.11582776699119467},{"quaternion_xyzw":[0.12350279984204965,-0.15450519512993105,-0.6006191254572018,0.774681785795989],"translation_xyz_m":[0.08345471734912846,0.057354184345051705,1.0154136628820487],"reprojection_error_px":0.255280343811108}]}]},{"stamp_ns":1789379972072340922,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08740124115483008}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08789393125487409,-0.3438139613988123,-0.308682017335674,0.8824862769297878],"translation_xyz_m":[0.06327163048225036,0.09615901817578092,1.0209427497276349],"reprojection_error_px":0.28526533545744487},{"quaternion_xyzw":[-0.18887520842526467,0.36711005411886616,-0.33348426583048035,0.8475521271583604],"translation_xyz_m":[0.0632081790378196,0.09615624279564224,1.0209257098762552],"reprojection_error_px":0.3219013559999396}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21885742694952784,0.18145594721878575,-0.6006671619568011,0.7472443552377811],"translation_xyz_m":[0.08304638163024408,0.05707347418314131,1.0152417528205944],"reprojection_error_px":0.11661337377341845},{"quaternion_xyzw":[0.12711560606083155,-0.15332416879900299,-0.6054016854382619,0.7705985473814456],"translation_xyz_m":[0.08309346039433632,0.057077229894915475,1.0151862484476288],"reprojection_error_px":0.26420342180546075}]}]},{"stamp_ns":1789379972105985733,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06762776458937378}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08859221086424841,-0.34385061053954225,-0.31083475071814587,0.8816461509878873],"translation_xyz_m":[0.06323940716946616,0.09610189785898673,1.0210109515007821],"reprojection_error_px":0.27098849608314923},{"quaternion_xyzw":[-0.18956270954504065,0.3668844507999685,-0.3355694306732194,0.8466728625087727],"translation_xyz_m":[0.06317579720232898,0.09609892206013265,1.0209913079647515],"reprojection_error_px":0.3122330458742085}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2215166868234655,0.1795173642631723,-0.6054802781026946,0.7430326414199057],"translation_xyz_m":[0.08269235207814676,0.05680505704562498,1.0152060171011201],"reprojection_error_px":0.1176567278762019},{"quaternion_xyzw":[0.1300137682129809,-0.15211078607037098,-0.6105128241460728,0.7663111772571907],"translation_xyz_m":[0.08273949863750246,0.05680911989154036,1.0151499492495208],"reprojection_error_px":0.2657137729025096}]}]},{"stamp_ns":1789379972139271174,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0958424438033377}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08953276462411622,-0.3435442837729489,-0.3134319500442425,0.8807506013833643],"translation_xyz_m":[0.06319211149443722,0.09605664198986653,1.0212311600103923],"reprojection_error_px":0.30100096093452744},{"quaternion_xyzw":[-0.19049302145423208,0.36625407283574407,-0.3380482971081341,0.8457503838194563],"translation_xyz_m":[0.06312885001129047,0.09605418498243391,1.0212168841773661],"reprojection_error_px":0.3316771942435324}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22498556916057735,0.1776113490550088,-0.6102008602686895,0.7385733629665385],"translation_xyz_m":[0.08229545613996697,0.05653297945595191,1.014997074258116],"reprojection_error_px":0.10643743077670477},{"quaternion_xyzw":[0.1337503462521929,-0.15096557036750416,-0.6155860537695632,0.7618228480726009],"translation_xyz_m":[0.08234289231779911,0.05653755637282037,1.0149420566633722],"reprojection_error_px":0.25826720499453254}]}]},{"stamp_ns":1789379972172278562,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04863696763543579}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09025931654293241,-0.3436715162780418,-0.31547608009209954,0.8798965777669996],"translation_xyz_m":[0.06310102894573287,0.09599127780038387,1.0212719008146507],"reprojection_error_px":0.27930901251996076},{"quaternion_xyzw":[-0.19118082229123218,0.36608383568719916,-0.34003718904961433,0.8448711312974501],"translation_xyz_m":[0.06303736316457234,0.09598824446598966,1.0212510603348777],"reprojection_error_px":0.32037456944831477}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22792746146194415,0.17606184396498087,-0.615085816829161,0.7339759787259855],"translation_xyz_m":[0.0818593368929219,0.056259126783206846,1.014850195666081],"reprojection_error_px":0.1018289432183097},{"quaternion_xyzw":[0.13699186067629554,-0.150218771738634,-0.6207511434028501,0.7571892555298424],"translation_xyz_m":[0.08190684540413956,0.056263913727617465,1.0147934827401046],"reprojection_error_px":0.2599245344675966}]}]},{"stamp_ns":1789379972205790609,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07608711899520486}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0910953199992077,-0.34366911330879235,-0.31797978147913236,0.8789095754412245],"translation_xyz_m":[0.06300208126293902,0.09588485577592847,1.02111007961165],"reprojection_error_px":0.28553600424874254},{"quaternion_xyzw":[-0.1919686438899075,0.3657421801170768,-0.3424557758503798,0.843862985936101],"translation_xyz_m":[0.06293879586683138,0.09588246303326321,1.0210955488380693],"reprojection_error_px":0.31683534351173437}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23085435539950888,0.1741082122708853,-0.6200863084013123,0.7293048519968492],"translation_xyz_m":[0.08142329481367372,0.05596540662214721,1.014672303657494],"reprojection_error_px":0.10545903278280569},{"quaternion_xyzw":[0.14022821432860974,-0.1490607417694195,-0.6260541001405076,0.7524448198151271],"translation_xyz_m":[0.08147088908970901,0.0559704872982602,1.014614581146894],"reprojection_error_px":0.2633873914161119}]}]},{"stamp_ns":1789379972239127147,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08812122256682345}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09206133640782033,-0.3438019093154825,-0.3203759099891211,0.8778862305495384],"translation_xyz_m":[0.06292318705766027,0.09578512147641749,1.0209377160306938],"reprojection_error_px":0.2857253989271783},{"quaternion_xyzw":[-0.19288977201372445,0.3655599981388376,-0.34477232295478666,0.8427879145654539],"translation_xyz_m":[0.06285992253109653,0.09578280733351957,1.0209236922866776],"reprojection_error_px":0.3158218500914841}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23398317554216838,0.17200734397139836,-0.6249847587206874,0.7246098250437624],"translation_xyz_m":[0.08101855684746909,0.05569599145661813,1.0147804634956852],"reprojection_error_px":0.11590176569972845},{"quaternion_xyzw":[0.14366816255997603,-0.14774527038914512,-0.6312761773177792,0.7476771911027716],"translation_xyz_m":[0.08106588903219401,0.0557011847475258,1.0147175272269282],"reprojection_error_px":0.27837108186225573}]}]},{"stamp_ns":1789379972272778526,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09638593446657417}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09289544277135077,-0.34374370321718056,-0.3226568777256784,0.8769853148526321],"translation_xyz_m":[0.06285398008606442,0.09569171689461255,1.020786172617589],"reprojection_error_px":0.2996335225389083},{"quaternion_xyzw":[-0.19368691411971278,0.36521288898437904,-0.34696421934145094,0.8418555431398217],"translation_xyz_m":[0.06279081351575504,0.09568959315522998,1.02077387104419],"reprojection_error_px":0.32591645223207377}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23686345241778187,0.1702498524799932,-0.6297380779578661,0.7199587806320598],"translation_xyz_m":[0.08060307740055354,0.055427716218013685,1.0145096077876774],"reprojection_error_px":0.10946056587754868},{"quaternion_xyzw":[0.14683866704789547,-0.14675421524955826,-0.63631075292258,0.742973910632926],"translation_xyz_m":[0.0806506142531377,0.0554332334826999,1.0144468108755518],"reprojection_error_px":0.2752215661950843}]}]},{"stamp_ns":1789379972305834727,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04286434370731997}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09387417765409482,-0.34362730348206805,-0.3250944144957724,0.8760259909015606],"translation_xyz_m":[0.06280364185518945,0.09565985059347672,1.0209298426629367],"reprojection_error_px":0.2954453631371869},{"quaternion_xyzw":[-0.1946645995294167,0.3647774561052741,-0.3493092962773617,0.840848450519849],"translation_xyz_m":[0.06274058418790858,0.09565789032676711,1.0209191694302013],"reprojection_error_px":0.31924600249453494}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23998522224986618,0.1682984657975434,-0.634635129175836,0.7150671103664643],"translation_xyz_m":[0.0801918762915795,0.05519749190512151,1.014361925919794],"reprojection_error_px":0.10499782667970067},{"quaternion_xyzw":[0.15023640917570466,-0.14561656401729534,-0.6415071824390206,0.7380334494593971],"translation_xyz_m":[0.08023915984524531,0.055203056182377384,1.0142934863916708],"reprojection_error_px":0.284795185849502}]}]},{"stamp_ns":1789379972339150519,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0826745158099666}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09467959120862449,-0.3434092929903944,-0.32735742182162114,0.8751816673552447],"translation_xyz_m":[0.06275632489681864,0.09563305576751012,1.0212205917562414],"reprojection_error_px":0.3206799244481918},{"quaternion_xyzw":[-0.19545399129711202,0.3642665860082596,-0.3514775547198047,0.8399828094265396],"translation_xyz_m":[0.06269323788142248,0.09563103893890557,1.0212091125635288],"reprojection_error_px":0.3435605874503508}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2425304683168016,0.16658974084751657,-0.6393643821914599,0.7103801918461149],"translation_xyz_m":[0.07978884518831304,0.05497724633042328,1.0143091458580376],"reprojection_error_px":0.1088941334027401},{"quaternion_xyzw":[0.15306103884071562,-0.14468797531523708,-0.6464756447450246,0.733285039353513],"translation_xyz_m":[0.07983616596614809,0.05498296478037067,1.0142390424216534],"reprojection_error_px":0.28926953127964466}]}]},{"stamp_ns":1789379972405663617,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04869664147170727}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09644454102819153,-0.34329083192575327,-0.3320590966582711,0.8732620520485697],"translation_xyz_m":[0.06260717446855013,0.09549216513646504,1.0213455884021359],"reprojection_error_px":0.3203556631502575},{"quaternion_xyzw":[-0.19714110042089836,0.3635182820735508,-0.35600351123083174,0.8380043825150623],"translation_xyz_m":[0.06254425226427011,0.09549044149178572,1.0213366670087543],"reprojection_error_px":0.33896910641801165}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2483055142939852,0.16250890761735853,-0.6488836465773734,0.700632028757366],"translation_xyz_m":[0.07891571122301765,0.054491235223132484,1.014006607852376],"reprojection_error_px":0.10662386878986374},{"quaternion_xyzw":[0.15946901763916987,-0.14220220236444164,-0.656550200743574,0.7233878627399505],"translation_xyz_m":[0.07896307016551818,0.05449740221501835,1.0139324872981896],"reprojection_error_px":0.2957027383216795}]}]},{"stamp_ns":1789379972472607133,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13122871427636543}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09825291130920472,-0.34307285123718306,-0.33661448732402705,0.8714000637404369],"translation_xyz_m":[0.06247001106929792,0.09534419352815467,1.0214740448511628],"reprojection_error_px":0.3441249999177173},{"quaternion_xyzw":[-0.19885894747939886,0.3627035684755758,-0.3603676566555242,0.8360839625654084],"translation_xyz_m":[0.06240743592001911,0.09534301648528652,1.021470634192587],"reprojection_error_px":0.35392775228763}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2528247250548876,0.1592954317600238,-0.6578012825373645,0.6913769568864054],"translation_xyz_m":[0.07815358368286866,0.05407612131729348,1.0142662049032038],"reprojection_error_px":0.10775156878959555},{"quaternion_xyzw":[0.1645833189477835,-0.14046522561942518,-0.6658765982844594,0.7139959435269452],"translation_xyz_m":[0.07820078141098494,0.0540823435343179,1.014186026631169],"reprojection_error_px":0.3069210591266869}]}]},{"stamp_ns":1789379972505978648,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08663656602870828}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0985637591935065,-0.343316648750174,-0.338388315597962,0.870581536635925],"translation_xyz_m":[0.0624278052565388,0.09530132914371627,1.0216301149705647],"reprojection_error_px":0.29156973906546874},{"quaternion_xyzw":[-0.19914833471150414,0.3627086768720389,-0.36212655222645673,0.8352524867822597],"translation_xyz_m":[0.06236468714725301,0.0952994113688883,1.0216177437534966],"reprojection_error_px":0.31522902125539054}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.25540387371362633,0.15762182575662498,-0.6624517751879968,0.6863540390261385],"translation_xyz_m":[0.07772404149256547,0.05384696141516346,1.0137985611566986],"reprojection_error_px":0.09491327937376792},{"quaternion_xyzw":[0.16745835155719846,-0.1395707587915246,-0.6707527848934863,0.7089205917038496],"translation_xyz_m":[0.07777160854331189,0.05385350854020036,1.0137203883016228],"reprojection_error_px":0.2988367232153965}]}]},{"stamp_ns":1789379972539247815,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07707681857419098}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09972761527373032,-0.3432089005928253,-0.34082402938539436,0.869540703072135],"translation_xyz_m":[0.06236312171675236,0.09523250474750436,1.0214664201750978],"reprojection_error_px":0.3053220960912696},{"quaternion_xyzw":[-0.20029147906816483,0.36228635328882935,-0.36445638873197744,0.8341483455272294],"translation_xyz_m":[0.06230033375683162,0.09523109157583785,1.021459593661257],"reprojection_error_px":0.32002599788660274}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2584053568792731,0.15430798937834897,-0.6672420169077212,0.6813250375723934],"translation_xyz_m":[0.0773622095193782,0.0537013176814873,1.0143925147805262],"reprojection_error_px":0.10672628681085806},{"quaternion_xyzw":[0.1707412582491043,-0.1370657626550676,-0.6758781465425664,0.7037393910145165],"translation_xyz_m":[0.07740992587559561,0.05370845115531785,1.0143159722893755],"reprojection_error_px":0.2986553658934809}]}]},{"stamp_ns":1789379972572614641,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04053730805241149}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10028872680636071,-0.3431564743382072,-0.34280621712097165,0.868717274433138],"translation_xyz_m":[0.06229485001286711,0.095200205789724,1.0216829556348803],"reprojection_error_px":0.29601217324226586},{"quaternion_xyzw":[-0.20082585824010207,0.3619530135537363,-0.36638199639737745,0.8333206005838133],"translation_xyz_m":[0.06223195329219257,0.09519865741890612,1.0216741408324954],"reprojection_error_px":0.31353835585222406}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.26201602536071184,0.1525625634735222,-0.6710406848904846,0.6765919493330315],"translation_xyz_m":[0.07689414270488876,0.05348540125759183,1.0135496272502573],"reprojection_error_px":0.1336071404828735},{"quaternion_xyzw":[0.17469238594984693,-0.1360497757385506,-0.6799742614846835,0.6990050304046451],"translation_xyz_m":[0.07694103199135931,0.05349216101933928,1.0134581534399116],"reprojection_error_px":0.33509437815824034}]}]},{"stamp_ns":1789379972605857340,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.058633689916268515}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10137434214742673,-0.343055520667066,-0.3450622267671194,0.867737409676765],"translation_xyz_m":[0.06220297403994669,0.09511817314172304,1.0215724215421316],"reprojection_error_px":0.3337348149176671},{"quaternion_xyzw":[-0.20185527579347876,0.361542343574513,-0.36853518166336374,0.8323000668709207],"translation_xyz_m":[0.06214045696141906,0.09511719994111216,1.0215698912805362],"reprojection_error_px":0.34086986764131927}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.26252378534555565,-0.15168131681575928,0.6758009748664144,-0.6718385837583918],"translation_xyz_m":[0.07656540380843826,0.05336648274535093,1.0147338136054274],"reprojection_error_px":0.13354882091638806},{"quaternion_xyzw":[0.17549246494953868,-0.13594276203248146,-0.6847456275503205,0.6941508378930568],"translation_xyz_m":[0.0766123404448867,0.05337302813338,1.0146422371984138],"reprojection_error_px":0.33459676533485655}]}]},{"stamp_ns":1789379972639090426,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07972892572015462}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10204143557710083,-0.34294749813328723,-0.34705218137811306,0.8669079203411264],"translation_xyz_m":[0.062145289339077044,0.09508375068845205,1.021795583028431],"reprojection_error_px":0.3262395846132233},{"quaternion_xyzw":[-0.2024951313377941,0.3611628638993227,-0.3704550982051282,0.8314566301006672],"translation_xyz_m":[0.06208268595747412,0.09508265715394153,1.0217914247820468],"reprojection_error_px":0.33525512983487743}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2661074395948452,-0.14887407940428823,0.6798122589613452,-0.6669922275706625],"translation_xyz_m":[0.07611837756953856,0.05316455870787283,1.0139298148777096],"reprojection_error_px":0.11266508710248355},{"quaternion_xyzw":[0.1793775473229811,-0.13387711437614738,-0.6891026062042533,0.6892301588621998],"translation_xyz_m":[0.07616566463463073,0.05317175423258508,1.013840218405076],"reprojection_error_px":0.323605995029686}]}]},{"stamp_ns":1789379972672566970,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.058645326563640625}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10316783168285297,-0.3426492555285756,-0.3494815390203857,0.8659160121371553],"translation_xyz_m":[0.06208566621416625,0.09502352644211084,1.0217823555055627],"reprojection_error_px":0.352537034550872},{"quaternion_xyzw":[-0.20359585598863508,0.3605560789979095,-0.3727618799268356,0.8304195458894976],"translation_xyz_m":[0.0620234417903615,0.0950229899165826,1.0217844100546705],"reprojection_error_px":0.3526333949858822}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.268023422288568,-0.1477404355883,0.6839044506946677,-0.6622770652203865],"translation_xyz_m":[0.07575802323832845,0.05303162885922005,1.0142469778330285],"reprojection_error_px":0.10200080597784583},{"quaternion_xyzw":[-0.18158488935831263,0.1334703523487614,0.6933250041509684,-0.6844801177679938],"translation_xyz_m":[0.07580560469761861,0.05303895441120322,1.014158922800507],"reprojection_error_px":0.31711404763721374}]}]},{"stamp_ns":1789379972705913965,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03929907138546457}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10354164433363716,-0.34289098459718387,-0.35110741869464435,0.8651176111421176],"translation_xyz_m":[0.06202842546346147,0.09496504393325644,1.0217114778339895],"reprojection_error_px":0.33222500532186866},{"quaternion_xyzw":[-0.2039364521637967,0.36057294465494155,-0.3743699868007778,0.8296048384890805],"translation_xyz_m":[0.061966044936051935,0.0949643558427933,1.0217110680763937],"reprojection_error_px":0.33539210471965075}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.26971641835748705,-0.1456631385444683,0.6880733444468622,-0.6577160302132629],"translation_xyz_m":[0.07540084827399963,0.05288917231593169,1.0142294081004128],"reprojection_error_px":0.12419131259301533},{"quaternion_xyzw":[-0.18354199327454085,0.13209620681986642,0.6976597217358521,-0.6798042670622662],"translation_xyz_m":[0.07544787505949463,0.052896225704090576,1.0141328309287674],"reprojection_error_px":0.33866945530837306}]}]},{"stamp_ns":1789379972739190582,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.038334205500067386}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20498474818594398,0.3600158316913627,-0.37655618194664375,0.8285983923352057],"translation_xyz_m":[0.061897440696631265,0.09489250110027113,1.021623855238415],"reprojection_error_px":0.34154496195722045},{"quaternion_xyzw":[0.10462360652521176,-0.34263178494131813,-0.3534086730755617,0.8641525737394725],"translation_xyz_m":[0.06195943886903349,0.09489261551103749,1.021617882110588],"reprojection_error_px":0.34717565469591716}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2715032877259827,-0.14409934268407965,0.6923016272665589,-0.6528704320736797],"translation_xyz_m":[0.07505101309159597,0.05276829419878209,1.0147613231975599],"reprojection_error_px":0.11049064814868022},{"quaternion_xyzw":[-0.18562303569800498,0.13127236846845947,0.7020259668288815,-0.6748860613414032],"translation_xyz_m":[0.07509838081579759,0.05277558152571414,1.014667691467059],"reprojection_error_px":0.3286590339235463}]}]},{"stamp_ns":1789379972772471819,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.051733152166642205}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20554509204170243,0.35985140302948115,-0.3784661685185484,0.8276602818561318],"translation_xyz_m":[0.06182968130843067,0.09482967332390937,1.0216695085187943],"reprojection_error_px":0.3404026204794753},{"quaternion_xyzw":[0.10523235173425274,-0.3427360398402705,-0.35536893756750376,0.8632329218439428],"translation_xyz_m":[0.0618917546490359,0.09482984979514311,1.0216648107474828],"reprojection_error_px":0.34481995000970783}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.27435853857608483,-0.14135806316416674,0.6962483527407668,-0.6480613563503375],"translation_xyz_m":[0.07464348075719496,0.052608800639106036,1.0145767501580802],"reprojection_error_px":0.10220841358148089},{"quaternion_xyzw":[-0.18879423168954498,0.12925499739837426,0.7062486113532124,-0.6699722253123332],"translation_xyz_m":[0.07469108164870468,0.05261658409195338,1.0144845490477976],"reprojection_error_px":0.3232682367013856}]}]},{"stamp_ns":1789379972805824697,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09560391261091582}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20622075052836164,0.3590738804400319,-0.38045602186683225,0.826917266637069],"translation_xyz_m":[0.061797490833399166,0.0948072492012974,1.0222152224151757],"reprojection_error_px":0.3255445331640613},{"quaternion_xyzw":[0.105934336011679,-0.34222276032413085,-0.357473559059441,0.8624813930417707],"translation_xyz_m":[0.06185932512640945,0.0948071034488591,1.0222070793709397],"reprojection_error_px":0.3349395537248226}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2763351827548281,-0.13916164002565884,0.7004027803213321,-0.6432020289422078],"translation_xyz_m":[0.07428308878104925,0.05248246499235287,1.015046158351128],"reprojection_error_px":0.10883636077925954},{"quaternion_xyzw":[-0.19108444754122242,0.12778940545422166,0.7105830930293694,-0.6650024583881904],"translation_xyz_m":[0.07433049580948559,0.05249025664430005,1.014950189863946],"reprojection_error_px":0.3311711514512326}]}]},{"stamp_ns":1789379972839381591,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10326305882988528}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10647782539733894,-0.34216560943725843,-0.3593668177661801,0.8616499629813097],"translation_xyz_m":[0.06179873463969241,0.09474627341789231,1.0221751316957313],"reprojection_error_px":0.3353035062873931},{"quaternion_xyzw":[-0.20673113689611475,0.358770958992274,-0.38228896136251744,0.8260755328913764],"translation_xyz_m":[0.06173642231139765,0.09474573789689349,1.022175409128781],"reprojection_error_px":0.3357838351741072}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2779946201346887,-0.13711566374734213,0.7043219911552062,-0.6386304241941982],"translation_xyz_m":[0.07391765655353011,0.05236146835263918,1.015129556219261],"reprojection_error_px":0.13113299208247975},{"quaternion_xyzw":[-0.19303461195738542,0.12643416281870473,0.7146523829666901,-0.660321143519444],"translation_xyz_m":[0.07396469393751055,0.05236910108460016,1.0150273281249373],"reprojection_error_px":0.3481379672172692}]}]},{"stamp_ns":1789379972872671920,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06351240109047876}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20762160850903577,0.35836363089798784,-0.3839729605112964,0.8252475636594887],"translation_xyz_m":[0.06167624220697226,0.0947153411147709,1.022120860143316],"reprojection_error_px":0.3604990112465894},{"quaternion_xyzw":[0.10739048419766226,-0.3420059195719212,-0.36113336622992276,0.8608611541224291],"translation_xyz_m":[0.061737623330402135,0.09471447097477001,1.0221051576830185],"reprojection_error_px":0.37911292565237525}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2795921736173773,-0.13599724674584618,0.7073034339246179,-0.6348659840375002],"translation_xyz_m":[0.0736414959016017,0.05229983864016245,1.015275210054364],"reprojection_error_px":0.1091836692611168},{"quaternion_xyzw":[-0.19484331390897805,0.12588603281782929,0.7177414817584152,-0.6565332856219785],"translation_xyz_m":[0.07368890833798544,0.052307713103838895,1.015176252675926],"reprojection_error_px":0.33574879985303974}]}]},{"stamp_ns":1789379972905816453,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08543968402445025}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20809386294724072,0.3581824106588191,-0.3856084806901719,0.8244443004340254],"translation_xyz_m":[0.061609781968290214,0.09468801917019282,1.0221230435693252],"reprojection_error_px":0.3517013328831804},{"quaternion_xyzw":[0.10788025832969887,-0.3420690382051685,-0.362801688789973,0.8600729954923895],"translation_xyz_m":[0.061671402489086445,0.09468746513012619,1.0221112792078668],"reprojection_error_px":0.3662741159906633}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28146349210111565,-0.13413512937464023,0.7107366864192703,-0.6305865779253866],"translation_xyz_m":[0.07330538460487937,0.05222821993728624,1.0154638562793739],"reprojection_error_px":0.12466721123681573},{"quaternion_xyzw":[-0.1969830920219432,0.12467741052879688,0.721327231435176,-0.6521811327778292],"translation_xyz_m":[0.07335257828349859,0.05223604490715353,1.0153603844316037],"reprojection_error_px":0.3472453418414031}]}]},{"stamp_ns":1789379972939210418,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08543216826105085}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20882516649188654,0.3578560030876453,-0.3871029081006384,0.8237004731295229],"translation_xyz_m":[0.061572994617909424,0.09466426810305748,1.0223274093129495],"reprojection_error_px":0.34686521313370644},{"quaternion_xyzw":[0.10863747669790697,-0.341951527848224,-0.3643679148128763,0.8593620156336289],"translation_xyz_m":[0.0616347551007887,0.09466395715569423,1.0223181561958588],"reprojection_error_px":0.3588689457073423}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28240322766580805,-0.1326482488419169,0.7140063612436847,-0.6267756976675622],"translation_xyz_m":[0.07304134378598057,0.05216777711241503,1.0159526249194077],"reprojection_error_px":0.10588699744309453},{"quaternion_xyzw":[-0.19814151841465097,0.12376621254364724,0.724674361744899,-0.6482815227527008],"translation_xyz_m":[0.07308905292567197,0.0521759761764034,1.0158557725838608],"reprojection_error_px":0.33007950160637484}]}]},{"stamp_ns":1789379972972657487,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04104183249627627}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20913388166313948,0.3578824705163443,-0.3881337984836905,0.8231253314728249],"translation_xyz_m":[0.061540281830309226,0.09461266628344006,1.0221502843103996],"reprojection_error_px":0.33671134943503195},{"quaternion_xyzw":[0.10896943744693502,-0.3421131348284521,-0.36541701666853127,0.8588100305710328],"translation_xyz_m":[0.06160219640224192,0.0946125353126973,1.022143392737815],"reprojection_error_px":0.3460946459417897}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28428267904186727,-0.13153726536699606,0.7160202965815768,-0.6238559457918005],"translation_xyz_m":[0.0728171277296963,0.05208598674574657,1.0156764430641199],"reprojection_error_px":0.11322295962111514},{"quaternion_xyzw":[-0.20020865510455818,0.12305336317014627,0.7268357730941479,-0.6453558113044094],"translation_xyz_m":[0.07286465095569611,0.05209413465906552,1.0155749025476333],"reprojection_error_px":0.3399871635058178}]}]},{"stamp_ns":1789379973006039236,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07488485795563046}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20975360084575076,0.3575302193938248,-0.3899612412237325,0.8222565290076393],"translation_xyz_m":[0.06148483825611152,0.09454112861711102,1.0220553923627762],"reprojection_error_px":0.3404162214966443},{"quaternion_xyzw":[0.1096284472192026,-0.34200315256351854,-0.36731806181850657,0.8579585588236863],"translation_xyz_m":[0.0615464132249534,0.0945404381301447,1.0220427626681778],"reprojection_error_px":0.3572944685298061}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28609742833489643,-0.12941238561872148,0.7194236322861005,-0.6195404210032659],"translation_xyz_m":[0.07251349706672637,0.05200063336607554,1.0160955289152624],"reprojection_error_px":0.12126667184623088},{"quaternion_xyzw":[-0.20230234163556218,0.1215476183900526,0.7304052736199697,-0.6409431139349355],"translation_xyz_m":[0.07256094502008859,0.05200889231544654,1.015991986522564],"reprojection_error_px":0.34486804688251876}]}]},{"stamp_ns":1789379973039359362,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09083417250520871}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21020643991158497,0.3571839486766547,-0.3913655243698146,0.8216239442483345],"translation_xyz_m":[0.06142834610941973,0.09450531184824551,1.0222428912877333],"reprojection_error_px":0.3460510438514261},{"quaternion_xyzw":[0.11012232107636775,-0.3418609225125941,-0.3687813913516535,0.8573240166076825],"translation_xyz_m":[0.061490065222291154,0.09450483992197803,1.022232835648034],"reprojection_error_px":0.359952821696305}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2871163007311114,-0.12767490624502584,0.7228019904214077,-0.6154840621922368],"translation_xyz_m":[0.07223246458922039,0.05194045884049054,1.0166657544382818],"reprojection_error_px":0.1022086222626398},{"quaternion_xyzw":[-0.20356617454393422,0.12041217324234042,0.7338720506545978,-0.6367837422427024],"translation_xyz_m":[0.07228045359348553,0.05194914971340012,1.016569410339456],"reprojection_error_px":0.3267446341727067}]}]},{"stamp_ns":1789379973072614963,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09649332631029146}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21085488736226723,0.35694029169936065,-0.39333746227950356,0.8206214019903094],"translation_xyz_m":[0.06135042646932815,0.09447285846868059,1.0222858208048722],"reprojection_error_px":0.3493183197935354},{"quaternion_xyzw":[0.11080380607255426,-0.3419134934438723,-0.37080198811282533,0.8563431351814639],"translation_xyz_m":[0.0614118883074083,0.09447194841707993,1.0222715241649432],"reprojection_error_px":0.36866933296405735}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2894528625898487,-0.12556889123802084,0.7265375020042841,-0.6104037615162371],"translation_xyz_m":[0.07182638180417697,0.051866387163373474,1.0166795469174927],"reprojection_error_px":0.13157169077280154},{"quaternion_xyzw":[-0.20623576395485965,0.11906114365875013,0.7377815755971012,-0.6316404043805974],"translation_xyz_m":[0.07187354357101079,0.05187457954138975,1.0165692299378084],"reprojection_error_px":0.3581645130376133}]}]},{"stamp_ns":1789379973105947159,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06295560532851585}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21141125698854732,0.357005924111085,-0.39478328298541493,0.8197549695142874],"translation_xyz_m":[0.06129877704919384,0.0944274797568103,1.0221633638621193],"reprojection_error_px":0.31393562129177227},{"quaternion_xyzw":[0.11138689293960129,-0.3421909841446115,-0.372267281944231,0.8555205206453311],"translation_xyz_m":[0.0613602794704636,0.09442657505866348,1.0221495708399992],"reprojection_error_px":0.33409668158445216}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2907203583866398,-0.12371264523395925,0.7297741420256703,-0.606305662400952],"translation_xyz_m":[0.0715603151203505,0.05183047790808567,1.0172653977239519],"reprojection_error_px":0.12172388710639416},{"quaternion_xyzw":[-0.2077407325667563,0.1178019888834481,0.7411284893457557,-0.6274512265728783],"translation_xyz_m":[0.07160784732710561,0.05183902002837381,1.0171597847254066],"reprojection_error_px":0.3467619565136413}]}]},{"stamp_ns":1789379973139600516,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03839694082958976}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2121667927482786,0.3562005646519387,-0.3966826236483323,0.8189928607088248],"translation_xyz_m":[0.06127265449680621,0.09440637821140366,1.0224626171870048],"reprojection_error_px":0.3513193985445721},{"quaternion_xyzw":[0.11215961026435997,-0.34162439673369105,-0.3742887938502273,0.8547636469690499],"translation_xyz_m":[0.061334075862411165,0.09440539726959785,1.022447804874432],"reprojection_error_px":0.3719705838017582}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2926186826010863,-0.12148291766143854,0.7333258666711534,-0.6015391762639158],"translation_xyz_m":[0.07124414521750148,0.05177515371135538,1.0175590980068605],"reprojection_error_px":0.116764810249512},{"quaternion_xyzw":[-0.20991200045043323,0.11624779349040716,0.7448442155481391,-0.6225997889013345],"translation_xyz_m":[0.07129183838435116,0.05178394683084869,1.0174545246136282],"reprojection_error_px":0.3431095360697119}]}]},{"stamp_ns":1789379973172781438,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07598368519837385}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21265659164862544,0.3560029727740149,-0.3982526307145576,0.8181894032150429],"translation_xyz_m":[0.06121626973202633,0.0943471328578763,1.0225111913323792],"reprojection_error_px":0.3438203645826064},{"quaternion_xyzw":[0.11269900389597731,-0.3416453562672252,-0.3759092608224816,0.8539728407214077],"translation_xyz_m":[0.061277813298269534,0.09434630374929158,1.0224984412353142],"reprojection_error_px":0.3625276865124856}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2941990706204958,-0.11952491431232329,0.7367939445113526,-0.5969048375043297],"translation_xyz_m":[0.07091257927117363,0.05170886378656879,1.0178969654082022],"reprojection_error_px":0.13023391748277335},{"quaternion_xyzw":[-0.2117944696990561,0.11494502448906777,0.7484375692909059,-0.6178769690027487],"translation_xyz_m":[0.07096003434130008,0.05171754871059556,1.0177877144954361],"reprojection_error_px":0.35391189114358146}]}]},{"stamp_ns":1789379973206302069,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1369660862893642}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2129950526468945,0.35565953632453795,-0.39948603457877907,0.8176492585124038],"translation_xyz_m":[0.06119547168732644,0.09433257476519644,1.0229743627655568],"reprojection_error_px":0.327843734595053},{"quaternion_xyzw":[0.11307185980508325,-0.34146841142155043,-0.3771980623501281,0.8534258610335003],"translation_xyz_m":[0.06125725593389806,0.0943321327005374,1.0229660560261498],"reprojection_error_px":0.3414992564531882}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2959385033643627,-0.11786018085146606,0.7392742567530168,-0.5932983678541476],"translation_xyz_m":[0.07065424333623709,0.05163938887357213,1.0179395296374842],"reprojection_error_px":0.12741632535936612},{"quaternion_xyzw":[-0.21377211656178133,0.11376555496799005,0.7510619409039668,-0.6142188873757364],"translation_xyz_m":[0.0707018352822224,0.05164828817172856,1.0178307521819328],"reprojection_error_px":0.3518522413875332}]}]},{"stamp_ns":1789379973239425365,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11473487860299303}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21347102468078819,0.35536166522089163,-0.4010079174255977,0.8169093332036024],"translation_xyz_m":[0.06112759001815449,0.09428178791497181,1.0227043810994108],"reprojection_error_px":0.34459727710428334},{"quaternion_xyzw":[0.11357031092210698,-0.3413897392573506,-0.37876825491862415,0.8526953966527288],"translation_xyz_m":[0.06118877807875826,0.09428037295412867,1.0226858578782612],"reprojection_error_px":0.370766851244654}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2972484104049108,-0.1162625819610835,0.7419716109718231,-0.5895799547638757],"translation_xyz_m":[0.07037555533420234,0.05160946482779145,1.018006955270316],"reprojection_error_px":0.12729806122619586},{"quaternion_xyzw":[-0.21530768973811243,0.11271032449123199,0.7538572767218114,-0.6104409781676622],"translation_xyz_m":[0.07042319133594285,0.051618443732128226,1.0178975955038043],"reprojection_error_px":0.35244614559383924}]}]},{"stamp_ns":1789379973272903309,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06043772687210002}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.213832669532415,0.3551833705069477,-0.4019850434505916,0.8164118982462168],"translation_xyz_m":[0.061083763897558645,0.09427161998192912,1.0227335738829453],"reprojection_error_px":0.3330155530818383},{"quaternion_xyzw":[0.11394302806075662,-0.3413650037457276,-0.37977256637752604,0.8522087293621514],"translation_xyz_m":[0.06114498238908828,0.09427023787338656,1.0227155594259068],"reprojection_error_px":0.3592788576391363}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2984371312796392,-0.1146861671645494,0.7443126719787333,-0.5863284131496298],"translation_xyz_m":[0.07016039990643724,0.05160977804964347,1.0184155741612044],"reprojection_error_px":0.11869842444655704},{"quaternion_xyzw":[-0.21668512585871694,0.11160986807119679,0.7562954523042266,-0.6071309433760296],"translation_xyz_m":[0.0702082938906105,0.05161903806448669,1.0183093494360025],"reprojection_error_px":0.3444523897391133}]}]},{"stamp_ns":1789379973306239148,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09200397234882857}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21486901969892627,0.35463636809890153,-0.4035995546385326,0.8155806215763228],"translation_xyz_m":[0.06101859438150757,0.09424254908375218,1.0225779693472534],"reprojection_error_px":0.3627318561040291},{"quaternion_xyzw":[0.11499424951657522,-0.3410548910533561,-0.38148760352996525,0.8514253297968454],"translation_xyz_m":[0.061079736114892404,0.09424109505444736,1.0225585446612924],"reprojection_error_px":0.3898593975969103}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.298969928480473,-0.11328897285641668,0.7474724544770366,-0.582295045738493],"translation_xyz_m":[0.06989181284235894,0.051628789335586685,1.0191023109497928],"reprojection_error_px":0.12778731628391177},{"quaternion_xyzw":[-0.21745031129108364,0.11082335267965078,0.7594769925165042,-0.6030159570511701],"translation_xyz_m":[0.06993952654528947,0.05163784371530531,1.018993085317706],"reprojection_error_px":0.3512268583077669}]}]},{"stamp_ns":1789379973339533228,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11979007994930256}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21513324243824486,0.35453373653073345,-0.40491710421548915,0.8149022373102841],"translation_xyz_m":[0.06097682748377767,0.09422270962321076,1.0226459937974302],"reprojection_error_px":0.3461524902652529},{"quaternion_xyzw":[0.11527484895560758,-0.34114129468209176,-0.38282215956779225,0.8507535015530292],"translation_xyz_m":[0.061038317126719775,0.09422173509247246,1.0226324271742881],"reprojection_error_px":0.3672227860360079}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30069295678288005,-0.11129550897449125,0.7504231520424828,-0.5779810968378661],"translation_xyz_m":[0.06960247047011561,0.05161577612458596,1.01942767116932],"reprojection_error_px":0.1349634985067353},{"quaternion_xyzw":[-0.21943435374231826,0.10944033304163352,0.7625629579539958,-0.598639384820177],"translation_xyz_m":[0.06965000151085012,0.05162480015906395,1.0193144979582767],"reprojection_error_px":0.35928043841885654}]}]},{"stamp_ns":1789379973372801258,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03628554751221854}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21576610779287778,0.3537944139631944,-0.4066639603207971,0.814186049224965],"translation_xyz_m":[0.06095640820326209,0.09419667544440583,1.0230873298913525],"reprojection_error_px":0.35262217996175704},{"quaternion_xyzw":[0.11594130727830992,-0.34062322813097523,-0.38468080829911455,0.850031826139804],"translation_xyz_m":[0.06101750144871981,0.0941951371325865,1.0230675477447768],"reprojection_error_px":0.3811726954414738}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30225698432427245,-0.1087171825373816,0.7535994337726732,-0.5735060444893642],"translation_xyz_m":[0.06933448469033632,0.05158464249608233,1.0201120415207114],"reprojection_error_px":0.1398716263594158},{"quaternion_xyzw":[-0.22127711974186237,0.10746774853627487,0.7658962302193688,-0.5940455233726193],"translation_xyz_m":[0.0693818751400218,0.051593785726589944,1.0199966220422017],"reprojection_error_px":0.36363320157046847}]}]},{"stamp_ns":1789379973406120125,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.074093092995985}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21607144738487968,0.35357070564436754,-0.40812074294103345,0.8134730142521054],"translation_xyz_m":[0.06092211634150391,0.09415662740291345,1.0232805601448527],"reprojection_error_px":0.3522149196339788},{"quaternion_xyzw":[0.11628891978995601,-0.3405926894294687,-0.3861806724181642,0.849316192764474],"translation_xyz_m":[0.060983096170857475,0.09415487686510347,1.023259054455018],"reprojection_error_px":0.3830244816601691}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3038155895292488,-0.10715323747318325,0.7563319376360146,-0.569364796391349],"translation_xyz_m":[0.06904926794265345,0.05153685742173067,1.0201658270216718],"reprojection_error_px":0.13876980156007424},{"quaternion_xyzw":[-0.22309988348286194,0.10645304787725123,0.7687394069695523,-0.5898592330036704],"translation_xyz_m":[0.06909680345138489,0.05154612105406215,1.0200507484397119],"reprojection_error_px":0.36221222200895153}]}]},{"stamp_ns":1789379973439376245,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13107231543433084}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2165192426458919,0.35340667252467584,-0.40913577864793726,0.81291515917052],"translation_xyz_m":[0.060866573298887616,0.09408796482046208,1.0230802268590302],"reprojection_error_px":0.3504124208540327},{"quaternion_xyzw":[0.11678240493465074,-0.34057232603943777,-0.38724182278978286,0.8487733097336487],"translation_xyz_m":[0.06092755558706364,0.09408619441126909,1.0230585411363784],"reprojection_error_px":0.3817672769395669}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30515068974142134,-0.10587407418035212,0.758470170172048,-0.5660359864231009],"translation_xyz_m":[0.06879471649794693,0.051483069455561935,1.01993171344133],"reprojection_error_px":0.13303142976331478},{"quaternion_xyzw":[-0.22465601969373972,0.10561381294077754,0.7709713299948489,-0.5864968914303387],"translation_xyz_m":[0.06884252077590784,0.05149254527643978,1.0198188683645182],"reprojection_error_px":0.35669289352040384}]}]},{"stamp_ns":1789379973472679549,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10490802795198055}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21701656667019945,0.3532906793293817,-0.4103589164801026,0.812216144480605],"translation_xyz_m":[0.06079858798774078,0.09404453517618154,1.0228598468987107],"reprojection_error_px":0.35785184109369755},{"quaternion_xyzw":[0.11731048669757958,-0.34064458616055593,-0.3884970736131739,0.8480976001759943],"translation_xyz_m":[0.06085961258143832,0.09404279075027808,1.0228386491474062],"reprojection_error_px":0.38856959277611297}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3057832336820119,-0.1044307934835074,0.7610885540965132,-0.5624366952774826],"translation_xyz_m":[0.06856827853937911,0.051510691233486845,1.0207225744960307],"reprojection_error_px":0.13449580852288007},{"quaternion_xyzw":[-0.22551130655567037,0.10469427645861545,0.7736281584371285,-0.5828234994966645],"translation_xyz_m":[0.06861608082141792,0.05152015955668081,1.0206095184756918],"reprojection_error_px":0.356669312115425}]}]},{"stamp_ns":1789379973506059632,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05724771024060291}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2173223546951405,0.35345517374513263,-0.4115585751895108,0.8114554661166178],"translation_xyz_m":[0.060741550759846455,0.09402306199680126,1.0227105806178787],"reprojection_error_px":0.34283943660245497},{"quaternion_xyzw":[0.11763522078421813,-0.3409999622663358,-0.3896849621239538,0.8473645088507417],"translation_xyz_m":[0.06080266909040505,0.0940213796509871,1.0226907902593612],"reprojection_error_px":0.3728241723254161}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3069586437341527,-0.10227655187908809,0.763842565761113,-0.5584446550053939],"translation_xyz_m":[0.0683200399555057,0.051536595515298704,1.0212194990575465],"reprojection_error_px":0.1452034354814546},{"quaternion_xyzw":[-0.2269148138941462,0.10311100714110662,0.7764884328195492,-0.5787430354994971],"translation_xyz_m":[0.06836754082218306,0.051545973659763766,1.0211016698176412],"reprojection_error_px":0.366969981202484}]}]},{"stamp_ns":1789379973539388849,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04598450692221953}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21784296950669496,0.3530705525729517,-0.4126614115738579,0.8109230450173474],"translation_xyz_m":[0.06072192260796218,0.09402172594639126,1.0229570926636948],"reprojection_error_px":0.3457058750418259},{"quaternion_xyzw":[0.11817076591683727,-0.3407689074711849,-0.39084998617892264,0.8468462139535483],"translation_xyz_m":[0.06078285551148697,0.09401979336747698,1.022934422604664],"reprojection_error_px":0.3791902448666754}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30787345833116303,-0.10099134911620393,0.7661141576304439,-0.5550529511110576],"translation_xyz_m":[0.06811860510443533,0.051549058241316816,1.0216454088503268],"reprojection_error_px":0.14393995572311322},{"quaternion_xyzw":[-0.2280246405320388,0.10229189481453506,0.7788210360470439,-0.5753076788780901],"translation_xyz_m":[0.06816618528936566,0.05155848742529636,1.0215279550513092],"reprojection_error_px":0.3657019457604513}]}]},{"stamp_ns":1789379973572872043,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03225125736120132}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21851739746989465,0.3526397659268953,-0.41394848136626533,0.8102727918821977],"translation_xyz_m":[0.06068734634207989,0.0939858210199219,1.022909889887427],"reprojection_error_px":0.36963748085342973},{"quaternion_xyzw":[0.11886869893315696,-0.34050814859130696,-0.3922170775972332,0.8462211278371303],"translation_xyz_m":[0.06074808787481041,0.09398361589229377,1.022883973316046],"reprojection_error_px":0.405810876390489}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3091183360278805,-0.09968252348861004,0.7679627574991493,-0.5520348285539597],"translation_xyz_m":[0.06790418757715268,0.051538183956998176,1.0217114502638838],"reprojection_error_px":0.14082654056234914},{"quaternion_xyzw":[-0.22946898801629137,0.10138822084447588,0.7807590926274276,-0.5722583782631468],"translation_xyz_m":[0.06795193993149967,0.05154778871002304,1.0215953134811768],"reprojection_error_px":0.36207292875891756}]}]},{"stamp_ns":1789379973606113284,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06146290989320773}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21867885232229048,0.3523145068923734,-0.41477089217691937,0.8099500940076196],"translation_xyz_m":[0.060678306801891214,0.09397581530964125,1.0231996499166554],"reprojection_error_px":0.3619609486545195},{"quaternion_xyzw":[0.11904751740921209,-0.3402860234103187,-0.3930820468153922,0.845883925454147],"translation_xyz_m":[0.06073924412257493,0.09397392037514314,1.02317728899009],"reprojection_error_px":0.3947130724069779}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3099151634777925,-0.09829034988607614,0.76961749091129,-0.5495275391181195],"translation_xyz_m":[0.06777015387603609,0.051531133136986804,1.0220794832595734],"reprojection_error_px":0.14310159234708045},{"quaternion_xyzw":[-0.2304143673228598,0.10031936882925761,0.7824937144355402,-0.5696918732418228],"translation_xyz_m":[0.06781784566778294,0.0515407978319143,1.0219623184474684],"reprojection_error_px":0.3642261239014558}]}]},{"stamp_ns":1789379973639539929,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06946727332621176}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2186132884503016,0.35240692838250604,-0.4150008664907649,0.8098097725718473],"translation_xyz_m":[0.060660576258048764,0.09395548247862716,1.0230681659318952],"reprojection_error_px":0.35280099585197905},{"quaternion_xyzw":[0.11898970706670176,-0.3404125450856552,-0.3932986733873402,0.8457404461607861],"translation_xyz_m":[0.060721651378603844,0.09395375245651127,1.0230479774129448],"reprojection_error_px":0.3835127217570345}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3102221267311051,-0.09809462958040592,0.770008667901204,-0.5488408941495945],"translation_xyz_m":[0.06771741992364085,0.051532685778216084,1.022173891720437],"reprojection_error_px":0.13584928174031874},{"quaternion_xyzw":[-0.23077412080773654,0.1002184121141566,0.7828995650135494,-0.5690060159087322],"translation_xyz_m":[0.06776534567018139,0.05154251755193095,1.022059860650059],"reprojection_error_px":0.3569429260375499}]}]},{"stamp_ns":1789379973672890784,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06262411058889751}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21902927548266327,0.352205287648354,-0.4162028329298257,0.8091679761923771],"translation_xyz_m":[0.06060759408758563,0.0939404436901303,1.0230768695475414],"reprojection_error_px":0.34540720892616705},{"quaternion_xyzw":[0.11942422285640876,-0.34039783661397555,-0.39453145560723296,0.8451107018374018],"translation_xyz_m":[0.060668611822195975,0.09393860006508968,1.0230556982995367],"reprojection_error_px":0.37784706407490737}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3109696301647867,-0.09623187782415629,0.7725376370560554,-0.5451815423668146],"translation_xyz_m":[0.06748280418090717,0.05156737141282203,1.0227285476336363],"reprojection_error_px":0.1304993730617377},{"quaternion_xyzw":[-0.2317389037307242,0.09888224343388344,0.7854933069723118,-0.5652606895343651],"translation_xyz_m":[0.06753090447334313,0.0515774172071879,1.0226171358483493],"reprojection_error_px":0.35077939165277666}]}]},{"stamp_ns":1789379973706203802,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08125919103736118}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21982699130927436,0.35159632952983183,-0.4181107673677394,0.8082323311797309],"translation_xyz_m":[0.06054832957003505,0.09391951195617851,1.023285319888757],"reprojection_error_px":0.3559139211789957},{"quaternion_xyzw":[0.12025514852050151,-0.3400639845136577,-0.3965331655699912,0.8441899278563141],"translation_xyz_m":[0.06060905273548587,0.0939172365051369,1.023259375042423],"reprojection_error_px":0.393697836219082}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31247668611084084,-0.09348379693642425,0.7762056278953285,-0.5395590084236898],"translation_xyz_m":[0.06712393124090443,0.05159627054969049,1.0232960387525705],"reprojection_error_px":0.12915986439836766},{"quaternion_xyzw":[-0.2335875082567093,0.09690795419094846,0.7892852639630236,-0.5595306037132974],"translation_xyz_m":[0.06717212317602848,0.051606502495278546,1.023185262847177],"reprojection_error_px":0.34850450018738593}]}]},{"stamp_ns":1789379973739565050,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.042118296561378006}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2199394704797739,0.3516539177897248,-0.41919036592369724,0.8076172289797754],"translation_xyz_m":[0.0604932910135472,0.09385174746582017,1.0228866781216246],"reprojection_error_px":0.3533203192385992},{"quaternion_xyzw":[0.12039590376764735,-0.34026843124567024,-0.3976134582989373,0.8435791360821706],"translation_xyz_m":[0.06055424479610229,0.09384971694861181,1.0228642131792072],"reprojection_error_px":0.38745993541140666}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31365391072281323,-0.09181528154568297,0.7787730669797495,-0.5354471855467419],"translation_xyz_m":[0.06687665624989868,0.05161750566531432,1.0236598912556625],"reprojection_error_px":0.13850681619487426},{"quaternion_xyzw":[-0.2350098945869737,0.09578874549468704,0.7919346374390542,-0.5553687024914594],"translation_xyz_m":[0.06692464459591725,0.05162759184972443,1.0235449222555728],"reprojection_error_px":0.35736944061310066}]}]},{"stamp_ns":1789379973772985068,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.046286033026090426}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2206744573962782,0.3515993100578728,-0.4201171791451137,0.8069586512373721],"translation_xyz_m":[0.06046186442124181,0.09381400908177018,1.0229055302072811],"reprojection_error_px":0.3434918957596855},{"quaternion_xyzw":[0.12117504845464443,-0.34034984689924663,-0.3985906119891838,0.8429733764370984],"translation_xyz_m":[0.06052276609186207,0.09381193142352372,1.022882194554546],"reprojection_error_px":0.3794270573203699}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31466150790251074,-0.09051765654903215,0.7806379320893511,-0.5323524286416389],"translation_xyz_m":[0.0666795097458392,0.051621416184385115,1.0239445500646038],"reprojection_error_px":0.13800496116065955},{"quaternion_xyzw":[-0.2362209563322645,0.0948955368882819,0.7938701012784345,-0.5522361443841968],"translation_xyz_m":[0.06672756468889317,0.05163157812929908,1.0238296800286124],"reprojection_error_px":0.35655166883574785}]}]},{"stamp_ns":1789379973806267421,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10112615042388848}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2209351146191545,0.3513343018957325,-0.42106846193255026,0.8065068095220373],"translation_xyz_m":[0.06043432807492843,0.09378694091621578,1.023008295587564],"reprojection_error_px":0.35004399628886285},{"quaternion_xyzw":[0.12146538515106685,-0.3402137833897432,-0.3995835269077445,0.8425163184328881],"translation_xyz_m":[0.06049487257527511,0.09378430118905573,1.0229790195302906],"reprojection_error_px":0.3926068985832886}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3148197647192813,-0.08921129778143543,0.7824839616549883,-0.5297628807711644],"translation_xyz_m":[0.06654232413225318,0.05163322646846946,1.0244896418759069],"reprojection_error_px":0.14611710997255883},{"quaternion_xyzw":[-0.2365342355310036,0.09393076946627468,0.7957435565910297,-0.5495641528639729],"translation_xyz_m":[0.06659012524187868,0.05164323900392427,1.0243713608278995],"reprojection_error_px":0.36376978609459615}]}]},{"stamp_ns":1789379973839455781,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09483064731945144}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22125315010848362,0.3510894387718373,-0.42241847159300283,0.8058200074501629],"translation_xyz_m":[0.0603977239726471,0.09377472927317636,1.0231585329214157],"reprojection_error_px":0.3674354341972121},{"quaternion_xyzw":[0.12180960157967163,-0.3401598583844239,-0.4009692726932732,0.8418297536097888],"translation_xyz_m":[0.06045790166952722,0.09377149190325465,1.0231232156311314],"reprojection_error_px":0.41558709381183884}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3164637545542217,-0.0873741887863261,0.7845707668626835,-0.525989690936883],"translation_xyz_m":[0.0662802469975975,0.0516364448050582,1.0243511939873182],"reprojection_error_px":0.15568208628820068},{"quaternion_xyzw":[-0.23842065149461086,0.09258950610459371,0.7979546379648998,-0.5457574296796136],"translation_xyz_m":[0.06632786332739808,0.05164640253982791,1.02422837407902],"reprojection_error_px":0.37345386264473146}]}]},{"stamp_ns":1789379973873013374,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05358225551124737}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22159719286420415,0.35075228265041325,-0.4229417849156797,0.8055977699215461],"translation_xyz_m":[0.06040008379796335,0.0938144358036684,1.023625500421574],"reprojection_error_px":0.36493497485196397},{"quaternion_xyzw":[0.12215291453085556,-0.33990361120477286,-0.401536671477727,0.8416130357950501],"translation_xyz_m":[0.06046039141698383,0.0938114726886804,1.0235928325261772],"reprojection_error_px":0.410516809896585}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31741313172301594,-0.08637766964173244,0.7858125293801532,-0.5237236586848228],"translation_xyz_m":[0.06614531557644032,0.05166290081011172,1.0245413954907163],"reprojection_error_px":0.15080425938932646},{"quaternion_xyzw":[-0.23950366730298833,0.09189697630149611,0.7992628197177669,-0.5434812638094033],"translation_xyz_m":[0.06619314877268459,0.051673059567772336,1.024421069893947],"reprojection_error_px":0.3681563926524349}]}]},{"stamp_ns":1789379973906663034,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06627755367608558}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22152695383917725,0.35081319731944455,-0.42353677753569413,0.8052779069264993],"translation_xyz_m":[0.06035811633763213,0.09380160228630513,1.0234011737717457],"reprojection_error_px":0.3457242510580799},{"quaternion_xyzw":[0.12207884743085885,-0.34006518326713603,-0.40210800068025004,0.8412856720094397],"translation_xyz_m":[0.060418520948826514,0.09379870087665378,1.023369880802756],"reprojection_error_px":0.39128551568780495}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3182025755210324,-0.08553085801297634,0.7870030808662312,-0.52159154897909],"translation_xyz_m":[0.06598701729725628,0.051674482196627916,1.0242504651241389],"reprojection_error_px":0.15839788393988602},{"quaternion_xyzw":[-0.24041745913865914,0.09134349759686122,0.800502587248295,-0.5413422379575321],"translation_xyz_m":[0.06603472600228956,0.05168453298949851,1.0241269868096694],"reprojection_error_px":0.37548119383032896}]}]},{"stamp_ns":1789379973939731256,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06143587444579996}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22173351066487085,0.35062569179327807,-0.42423315718435056,0.8049360861882854],"translation_xyz_m":[0.06033789195376909,0.09378511071062184,1.0234033299412928],"reprojection_error_px":0.35643022207669334},{"quaternion_xyzw":[0.12229844742357503,-0.3399705294353441,-0.40283310478459955,0.8409450746409272],"translation_xyz_m":[0.060398305954597,0.09378221276927809,1.0233722025522067],"reprojection_error_px":0.40114347995521}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31785306361457805,-0.08437695248701324,0.7886427029466144,-0.5195119314596051],"translation_xyz_m":[0.06590870430364426,0.05174139995494356,1.0254103121400575],"reprojection_error_px":0.15130906730077884},{"quaternion_xyzw":[-0.240183140964245,0.09049134644881981,0.8021324457270841,-0.5391724348724775],"translation_xyz_m":[0.06595650207888425,0.0517515742966869,1.0252897989335163],"reprojection_error_px":0.36789119718052304}]}]},{"stamp_ns":1789379973972806491,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06624756034762803}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22222492420412096,0.35026208633347455,-0.4249835984045623,0.8045629217326038],"translation_xyz_m":[0.060329410723012435,0.09375195255622433,1.0235876899682492],"reprojection_error_px":0.3397845739492066},{"quaternion_xyzw":[0.12282016280584969,-0.3396962623870183,-0.4036545581627772,0.8405858995980409],"translation_xyz_m":[0.06039004404513759,0.09374944646571275,1.0235604182736533],"reprojection_error_px":0.381433435139884}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.318987534497985,-0.08323757623067025,0.7899285997449356,-0.5170408746348255],"translation_xyz_m":[0.06579064225944117,0.05173599591266794,1.0257867631327202],"reprojection_error_px":0.16068316579742692},{"quaternion_xyzw":[-0.24148085987033457,0.08963503943286309,0.8035090082623702,-0.5366803775650598],"translation_xyz_m":[0.06583824155156126,0.051746079775489395,1.0256623434896828],"reprojection_error_px":0.37661875806436806}]}]},{"stamp_ns":1789379974006457089,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13913335895990334}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22259402234753328,0.34990894112643556,-0.4256778127762521,0.8042474953923999],"translation_xyz_m":[0.06029790451595387,0.09371059042153915,1.0235130376213302],"reprojection_error_px":0.3569279774135799},{"quaternion_xyzw":[0.12321793360632978,-0.33943338102173126,-0.4044081023884801,0.8402716271590343],"translation_xyz_m":[0.06035843123223995,0.0937079376860752,1.0234838845618965],"reprojection_error_px":0.3997672296070127}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31998039342152873,-0.08202051313571057,0.7909236977220286,-0.5150969691548102],"translation_xyz_m":[0.06562990891804586,0.05170969553435242,1.0254787863401678],"reprojection_error_px":0.15095273412669616},{"quaternion_xyzw":[-0.2426183482296837,0.08865853294049984,0.8045904582085373,-0.5347057098993],"translation_xyz_m":[0.06567783533852077,0.05172014355664182,1.0253585374512282],"reprojection_error_px":0.36694237341272967}]}]},{"stamp_ns":1789379974039586690,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13564865215015143}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22284742440524752,0.3500314611185277,-0.42632286441968087,0.8037821949609076],"translation_xyz_m":[0.06025375940392086,0.09366930898728641,1.0231988566587902],"reprojection_error_px":0.3374306442401458},{"quaternion_xyzw":[0.12349099256768666,-0.3396593858714208,-0.405050652402259,0.8398306051416566],"translation_xyz_m":[0.06031430275328824,0.09366662280181415,1.0231696383308992],"reprojection_error_px":0.38185544472925786}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32001559076656183,-0.08134812180750331,0.7922223042541134,-0.5131825458713722],"translation_xyz_m":[0.06553634738695384,0.05174690431025703,1.026090728781955],"reprojection_error_px":0.1552664631952439},{"quaternion_xyzw":[-0.24277063463922754,0.08824393718779051,0.8058858616145776,-0.5327507902914774],"translation_xyz_m":[0.0655840904232652,0.05175717431957036,1.025967828998645],"reprojection_error_px":0.37156473804011936}]}]},{"stamp_ns":1789379974073145721,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04582288631431477}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22277272161888634,0.35003780268333984,-0.4269777924560158,0.8034524353964713],"translation_xyz_m":[0.06023421780463335,0.09370240866396423,1.0234100183694614],"reprojection_error_px":0.32439119916845316},{"quaternion_xyzw":[0.12340763058555967,-0.3397764636562024,-0.40568408060133815,0.8394896891595455],"translation_xyz_m":[0.060294886970808925,0.09369986963687936,1.0233831264065496],"reprojection_error_px":0.36700295601340105}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3211782683246707,-0.07980692596400282,0.7938783993082913,-0.510129847819011],"translation_xyz_m":[0.06534801101409812,0.051801449306775785,1.0262289940029354],"reprojection_error_px":0.16451714804474524},{"quaternion_xyzw":[-0.244110223260816,0.0871222341340347,0.8076299192394356,-0.5296733226889491],"translation_xyz_m":[0.06539554486833708,0.05181162494518181,1.0261018613605228],"reprojection_error_px":0.3806338159887425}]}]},{"stamp_ns":1789379974106449314,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09557926198918053}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22280179362232772,0.3498722546593329,-0.427622027431956,0.8031738092301391],"translation_xyz_m":[0.06022810550548209,0.09374706237265522,1.0236963320997285],"reprojection_error_px":0.33563652309697695},{"quaternion_xyzw":[0.12342389661331622,-0.33971355168715694,-0.4063307288894656,0.8391999662208277],"translation_xyz_m":[0.06028825889467194,0.0937437129975728,1.023660987426162],"reprojection_error_px":0.38741793197879604}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3219053789290342,-0.07933634290371964,0.7947545956269402,-0.5083776199254616],"translation_xyz_m":[0.0652225543479229,0.051832368910060655,1.0261064202338022],"reprojection_error_px":0.15860498029690057},{"quaternion_xyzw":[-0.24493761971377678,0.08689979008974835,0.8085373029734482,-0.5279407340144625],"translation_xyz_m":[0.06527040768827334,0.051842738317457525,1.0259830199147741],"reprojection_error_px":0.3735016253389231}]}]},{"stamp_ns":1789379974172995652,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04550379453388527}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22242820491482856,0.35007383251744206,-0.4281682798833861,0.8028984553146302],"translation_xyz_m":[0.060193368080234746,0.09368571574985743,1.0234769358525773],"reprojection_error_px":0.32158822824622707},{"quaternion_xyzw":[0.12308415319330357,-0.3399827506436367,-0.40684336957310024,0.8388924204758794],"translation_xyz_m":[0.06025363461412219,0.09368241107876289,1.0234432660146178],"reprojection_error_px":0.372699254128688}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32305519030712654,-0.07804116747020497,0.796477101105148,-0.5051427002448452],"translation_xyz_m":[0.0650036587213208,0.05181866897997246,1.0258569666285942],"reprojection_error_px":0.15824021546784575},{"quaternion_xyzw":[-0.24629798541660733,0.08600064128553167,0.8103386109345244,-0.5246832641764969],"translation_xyz_m":[0.06505161954352225,0.05182911836526667,1.025733614933413],"reprojection_error_px":0.3730822048534377}]}]},{"stamp_ns":1789379974239778698,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12163354937710256}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22242484041252328,0.3499368179040882,-0.42904767215153056,0.802489569317868],"translation_xyz_m":[0.060160553265368344,0.09364762716249511,1.0234940154463095],"reprojection_error_px":0.3299360299893846},{"quaternion_xyzw":[0.12311386591642913,-0.3399606706164895,-0.40773927411459876,0.8384619268622746],"translation_xyz_m":[0.06022085882405872,0.09364432678733582,1.0234610209359614],"reprojection_error_px":0.37960096506300217}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32314231855048686,-0.07649590264450151,0.7985560575296468,-0.5020315147708618],"translation_xyz_m":[0.06483930587607221,0.051866213413223956,1.0267546328507617],"reprojection_error_px":0.16187638325654868},{"quaternion_xyzw":[-0.24658400114461398,0.08487397979886056,0.8124349314413287,-0.5214807955299399],"translation_xyz_m":[0.06488717023684294,0.051876634357372144,1.0266305160061837],"reprojection_error_px":0.37518092897015065}]}]},{"stamp_ns":1789379974306260272,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10658797684917583}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22260124705883985,0.34978520721296485,-0.4296296579018958,0.8021953319946039],"translation_xyz_m":[0.0601278058030053,0.09367890541980119,1.0234819034539009],"reprojection_error_px":0.3300981865040483},{"quaternion_xyzw":[0.12327064481101638,-0.33991891401550234,-0.40832348678275493,0.8381714682354507],"translation_xyz_m":[0.06018796563854138,0.09367536185187723,1.0234464061517685],"reprojection_error_px":0.3827909591964202}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32385551469960405,-0.07548646280554708,0.7999713850674304,-0.4994648962690553],"translation_xyz_m":[0.06466633825346407,0.05193445358616634,1.0267611550481401],"reprojection_error_px":0.16786420697400653},{"quaternion_xyzw":[-0.24744110269002845,0.08423674887236803,0.8138876301353786,-0.5189065391297103],"translation_xyz_m":[0.06471403263781518,0.05194471060167708,1.0266333323847563],"reprojection_error_px":0.38217500613189337}]}]},{"stamp_ns":1789379974339908060,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05969564351824247}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22252382462251785,0.3497378455184721,-0.43000275422077505,0.8020375416712188],"translation_xyz_m":[0.06012950743755063,0.09366255556174145,1.023446253020805],"reprojection_error_px":0.3326861486211027},{"quaternion_xyzw":[0.12319580638212344,-0.3399069135808589,-0.40870005391330944,0.8380037883691522],"translation_xyz_m":[0.06018960286751312,0.09365887308320442,1.0234096310449965],"reprojection_error_px":0.38647450922472953}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32363788547105593,-0.07493429926152004,0.8011120666971889,-0.49785823933542317],"translation_xyz_m":[0.06460019798670877,0.05196490841283413,1.0271537346928703],"reprojection_error_px":0.16642864684305955},{"quaternion_xyzw":[-0.24729924674017617,0.08389853849192301,0.8150134295657877,-0.5172593425237225],"translation_xyz_m":[0.06464795804210792,0.051975177277746754,1.0270273275725095],"reprojection_error_px":0.37964858691743936}]}]},{"stamp_ns":1789379974373016569,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07404763651851679}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22233658620298677,0.349644677061257,-0.4301209260454752,0.8020667249151494],"translation_xyz_m":[0.06013249198040332,0.0936277250811315,1.0234332045596524],"reprojection_error_px":0.33270574261193764},{"quaternion_xyzw":[0.12302392810399532,-0.3398057079752161,-0.4088287134561892,0.8380073251440482],"translation_xyz_m":[0.060192866335135084,0.09362444957826346,1.0234013021374408],"reprojection_error_px":0.3809676985602832}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32358917086630123,-0.07470765118261331,0.80137594331599,-0.4974991586196338],"translation_xyz_m":[0.06459464266080739,0.05194886341758166,1.027349366950685],"reprojection_error_px":0.16697120311730007},{"quaternion_xyzw":[-0.24727967035315804,0.08369666169076892,0.8152830540578494,-0.5168763635705935],"translation_xyz_m":[0.06464235626763619,0.051959118610158964,1.0272225310851597],"reprojection_error_px":0.3803122142085177}]}]},{"stamp_ns":1789379974406653799,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12012066601480559}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22236510697389478,0.34946857819293803,-0.4305110696470401,0.8019262378596027],"translation_xyz_m":[0.0601234869222384,0.09362729761100096,1.0236626164500815],"reprojection_error_px":0.34169294218869684},{"quaternion_xyzw":[0.12307154718828313,-0.33968391850470014,-0.40923875424305617,0.8378495520125113],"translation_xyz_m":[0.0601836343366474,0.0936236788953326,1.0236270874280646],"reprojection_error_px":0.393411416558991}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3240968885867829,-0.07405379880791375,0.8019949201785479,-0.4962674578272163],"translation_xyz_m":[0.06448939022703248,0.05194122974773657,1.0272284311390865],"reprojection_error_px":0.16144843107310905},{"quaternion_xyzw":[-0.24788811851276038,0.08319495619561394,0.8159401203510285,-0.5156275787478426],"translation_xyz_m":[0.0645371980154759,0.05195159408942818,1.027102492891465],"reprojection_error_px":0.3765001604294456}]}]},{"stamp_ns":1789379974439773464,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10162165325838077}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22248227550684319,0.3493528943400059,-0.430701129316948,0.8018420851431182],"translation_xyz_m":[0.060098174886891675,0.09361914666271659,1.0235719978187179],"reprojection_error_px":0.3436976349872542},{"quaternion_xyzw":[0.12319361117048587,-0.33960708852787014,-0.4094413345819708,0.8377637812200603],"translation_xyz_m":[0.06015827281670268,0.09361545104408556,1.023535520691737],"reprojection_error_px":0.39637181333003696}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3243421881230917,-0.07329613344995303,0.8026418503570211,-0.4951725778759051],"translation_xyz_m":[0.06440714595466543,0.051950795485706726,1.0271247185277192],"reprojection_error_px":0.16754463536567907},{"quaternion_xyzw":[-0.24819980129156408,0.0825933106020143,0.8166157272230294,-0.5145036032280339],"translation_xyz_m":[0.06445486365911997,0.05196114720492011,1.0269972727167564],"reprojection_error_px":0.38119227866241145}]}]},{"stamp_ns":1789379974473142059,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04357028226948803}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222937784460787,0.34919860037471007,-0.43113201116351885,0.8017300059936691],"translation_xyz_m":[0.06009434446569405,0.09363621899874325,1.0236268438549385],"reprojection_error_px":0.3432003866494191},{"quaternion_xyzw":[0.12298443225304286,-0.3395094828251888,-0.4098668234658765,0.8376260069489017],"translation_xyz_m":[0.06015449648366645,0.09363256233962826,1.023591336354848],"reprojection_error_px":0.3948281087168508}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32429579505926487,-0.07287934503256381,0.8038189941086975,-0.49335166269567343],"translation_xyz_m":[0.06431776564895544,0.05200300811555464,1.027408299631667],"reprojection_error_px":0.16906385454357845},{"quaternion_xyzw":[-0.24823809346436887,0.08243355526370631,0.8177742834355445,-0.512667318317876],"translation_xyz_m":[0.06436549187866783,0.05201327030614217,1.0272807844988547],"reprojection_error_px":0.3818396034904932}]}]},{"stamp_ns":1789379974539824274,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05695619404058862}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22229454467273135,0.3490603974421344,-0.4315024946157591,0.8015906508228866],"translation_xyz_m":[0.060085734011205795,0.09364075274735699,1.023719463250051],"reprojection_error_px":0.3422318474668127},{"quaternion_xyzw":[0.12298607336607074,-0.33942502865999996,-0.4102461487145672,0.8374742820779745],"translation_xyz_m":[0.060145654546004096,0.09363672475519107,1.0236801002451528],"reprojection_error_px":0.3983340465219834}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3243515430246777,-0.07212693593420762,0.8048280361317693,-0.49177801283356787],"translation_xyz_m":[0.06423414832564224,0.05204685284670534,1.027750643309609],"reprojection_error_px":0.1603839347797006},{"quaternion_xyzw":[-0.2483793584945408,0.0819019550634224,0.8187914652356213,-0.5110580206668651],"translation_xyz_m":[0.06428207186059527,0.05205728767067003,1.0276265344615818],"reprojection_error_px":0.3734850588047715}]}]},{"stamp_ns":1789379974606388323,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13373565072415483}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.222077805762215,0.3491954526298791,-0.43182854159174816,0.8014163054974596],"translation_xyz_m":[0.06009092233571695,0.0935779348773977,1.0235684811682477],"reprojection_error_px":0.33709162317730545},{"quaternion_xyzw":[0.12279824552231104,-0.33956838008492274,-0.4105661098710488,0.8372869135299426],"translation_xyz_m":[0.06015105219454994,0.09357415644016767,1.0235325352344418],"reprojection_error_px":0.3898148843788146}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.324425395393428,-0.07113134088762456,0.8054392564672126,-0.49087279340873036],"translation_xyz_m":[0.06421413802330847,0.05203136030526583,1.0280990441928772],"reprojection_error_px":0.1609365625780692},{"quaternion_xyzw":[-0.2485119570103882,0.08098757662916481,0.8194486584784293,-0.5100850083785101],"translation_xyz_m":[0.06426191393035662,0.05204183045059829,1.0279735334270135],"reprojection_error_px":0.37518629873614795}]}]},{"stamp_ns":1789379974639816270,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07328100327109056}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222417334089391,0.3489312648388575,-0.4318423713901042,0.8014784779527871],"translation_xyz_m":[0.06007152185507515,0.09359940318428475,1.0235935375492067],"reprojection_error_px":0.34774681820778863},{"quaternion_xyzw":[0.12295140027507105,-0.3393269064602795,-0.41060324032810835,0.8373441244521594],"translation_xyz_m":[0.060131487909296885,0.09359541237647437,1.0235548182247949],"reprojection_error_px":0.4026503218062637}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32438456105011826,-0.07114904004439619,0.8054726407956366,-0.49084243457833443],"translation_xyz_m":[0.06419056547849149,0.052048985951010915,1.028109819610123],"reprojection_error_px":0.1676361453779959},{"quaternion_xyzw":[-0.2484824170096148,0.08103613683680955,0.8194694652119001,-0.5100582599556135],"translation_xyz_m":[0.06423819093878227,0.052059318734323366,1.0279818484606988],"reprojection_error_px":0.3812711208697004}]}]},{"stamp_ns":1789379974673074828,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09329154517289591}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22245931357202384,0.34877195387275384,-0.4320127921633188,0.8013956110461915],"translation_xyz_m":[0.06005848592273214,0.09362117573845936,1.0236569233773463],"reprojection_error_px":0.35236038219147797},{"quaternion_xyzw":[0.1231613355505322,-0.3392085438885846,-0.4107909920241113,0.8372691383600867],"translation_xyz_m":[0.060118307846516214,0.09361699308868121,1.0236158069805337],"reprojection_error_px":0.40951883244408416}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32470604180846935,-0.07090724441402609,0.8057886210263309,-0.4901457409048561],"translation_xyz_m":[0.06414471780171835,0.052073107806914196,1.028059638354481],"reprojection_error_px":0.1630167352272991},{"quaternion_xyzw":[-0.24883790798756295,0.08089602350422198,0.8198025497647613,-0.5093714836234685],"translation_xyz_m":[0.06419251427227406,0.05208356614986474,1.0279339177264482],"reprojection_error_px":0.3763945308978179}]}]},{"stamp_ns":1789379974706641943,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13596845948031958}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22217749698135716,0.3488186582166257,-0.4324016988181594,0.8012437047329743],"translation_xyz_m":[0.06004353601799832,0.09362645659003298,1.0235157483013086],"reprojection_error_px":0.34682544432791435},{"quaternion_xyzw":[0.12285967791633569,-0.33931245079883526,-0.41114818392995295,0.8370960106978673],"translation_xyz_m":[0.06010336623278279,0.09362219737581268,1.0234746274851538],"reprojection_error_px":0.4045413465318147}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32478831500264216,-0.07030727472362765,0.8065573392684323,-0.4889117466689751],"translation_xyz_m":[0.06407628692715181,0.052118485358262634,1.028246331666414],"reprojection_error_px":0.15869030882062277},{"quaternion_xyzw":[-0.2489791922746432,0.08048052723380877,0.8205791523371587,-0.5081162281408304],"translation_xyz_m":[0.06412422882717965,0.05212907086010881,1.0281230437866131],"reprojection_error_px":0.371340756731491}]}]},{"stamp_ns":1789379974773236397,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04596385773337468}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.222270347436101,0.34885575119001894,-0.4322727962632726,0.8012713567344174],"translation_xyz_m":[0.06008250117738869,0.09361433221934222,1.0237662063465782],"reprojection_error_px":0.34856365765525793},{"quaternion_xyzw":[0.12297415876732455,-0.33930202174187624,-0.41104024226994174,0.8371364366410124],"translation_xyz_m":[0.06014243107392727,0.09361027695433613,1.023727055718997],"reprojection_error_px":0.4039012272559606}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3252261528250968,-0.07044940809123523,0.8065797976614754,-0.48856305675168993],"translation_xyz_m":[0.06405197445073699,0.05206183696621662,1.027748570365608],"reprojection_error_px":0.16554339658676262},{"quaternion_xyzw":[-0.24944397040193111,0.08061683388573264,0.8206264537061156,-0.5077901684771823],"translation_xyz_m":[0.06409975872598919,0.052072236504970934,1.027621549508901],"reprojection_error_px":0.37914724488570284}]}]},{"stamp_ns":1789379974806470295,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11654187295312257}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2223459550875239,0.3488263315369118,-0.4323680763790464,0.8012117780031697],"translation_xyz_m":[0.060066399156436065,0.09357692267077022,1.0236222299871407],"reprojection_error_px":0.340489236749988},{"quaternion_xyzw":[0.12307049824951097,-0.3392835592629459,-0.41114857977919594,0.837076558159428],"translation_xyz_m":[0.06012618293512614,0.09357263136897435,1.0235804223320626],"reprojection_error_px":0.39964310749773224}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32508973023805,-0.07069311349799812,0.8066274329483362,-0.48854000390232977],"translation_xyz_m":[0.06404432138551877,0.05203868837199648,1.0277729258581882],"reprojection_error_px":0.16795427689732315},{"quaternion_xyzw":[-0.2493250877054118,0.08085077847923645,0.8206529754934481,-0.5077684965353209],"translation_xyz_m":[0.0640920907505568,0.05204901726401674,1.0276455104489959],"reprojection_error_px":0.38076649537860574}]}]},{"stamp_ns":1789379974873282598,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07484558108273015}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22221388006663176,0.3486174973813631,-0.43241569684762515,0.801313607238184],"translation_xyz_m":[0.06005217120192704,0.09360041910634082,1.0236961783969456],"reprojection_error_px":0.3433146596005532},{"quaternion_xyzw":[0.12292280672340217,-0.3390957986389651,-0.4111975296933408,0.8371502938581207],"translation_xyz_m":[0.060111872299295196,0.09359599775056089,1.0236530209478074],"reprojection_error_px":0.40364563210100585}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32520543143021324,-0.07026320451666115,0.8067223935770197,-0.4883681901605619],"translation_xyz_m":[0.06402943032753021,0.05206047909774616,1.0278639793434161],"reprojection_error_px":0.1702509359376658},{"quaternion_xyzw":[-0.24945100422184222,0.08046249995063287,0.8207688226953362,-0.5075810499671167],"translation_xyz_m":[0.06407715859893612,0.05207085652370759,1.0277362509199686],"reprojection_error_px":0.3821525091738253}]}]},{"stamp_ns":1789379974906442864,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.052672071359291625}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22227826431425307,0.3486134362985646,-0.4323900457150166,0.801311358719016],"translation_xyz_m":[0.06006252303227358,0.09362035255413821,1.0237145758080788],"reprojection_error_px":0.3469082899183147},{"quaternion_xyzw":[0.12297067473420116,-0.33908842602917183,-0.411171581711377,0.8371589949832939],"translation_xyz_m":[0.060122278846249905,0.09361602890746676,1.0236723820519376],"reprojection_error_px":0.4058007068151377}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3249383692530909,-0.07014638580546612,0.8068170891353061,-0.48840631181916977],"translation_xyz_m":[0.06404231343865732,0.05208380511242749,1.0279343799255327],"reprojection_error_px":0.16278442867919393},{"quaternion_xyzw":[-0.24915869611196223,0.08035936183318972,0.8208531182048077,-0.5076046448280327],"translation_xyz_m":[0.06409023528386876,0.052094360858392114,1.0278102945546048],"reprojection_error_px":0.3742914935857905}]}]},{"stamp_ns":1789379974939891843,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0381640490337545}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22231588367371447,0.34866964560519786,-0.4323225002858535,0.8013129113189863],"translation_xyz_m":[0.06006838177323849,0.09360543676171748,1.0235759434120537],"reprojection_error_px":0.342428901528285},{"quaternion_xyzw":[0.12300668566553072,-0.33912659502104464,-0.4111045653439069,0.8371711558483195],"translation_xyz_m":[0.06012815468363431,0.09360113309429187,1.0235339011588633],"reprojection_error_px":0.4016370511045409}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32479143338324873,-0.07019714231573866,0.8067977321915691,-0.48852871496166905],"translation_xyz_m":[0.06405906000500032,0.05208380788810039,1.027997744168901],"reprojection_error_px":0.16352749865405317},{"quaternion_xyzw":[-0.24899905111938417,0.08039238308824369,0.8208259010154021,-0.5077217520505622],"translation_xyz_m":[0.06410693352321925,0.05209432374038373,1.0278731190863633],"reprojection_error_px":0.37529030301434846}]}]},{"stamp_ns":1789379974973104278,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.047506951497346496}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2221841305722405,0.34870069156239286,-0.4322856648488055,0.801355815971902],"translation_xyz_m":[0.06006796735944389,0.09359460929019917,1.0236024165192517],"reprojection_error_px":0.3388347070049978},{"quaternion_xyzw":[0.12288388878254115,-0.3391476188581622,-0.41106179533749704,0.8372016739797122],"translation_xyz_m":[0.060127894837519714,0.09359053168219152,1.0235630259087691],"reprojection_error_px":0.39540188065156306}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32485559130080366,-0.07041397961540735,0.8067813120964635,-0.48848196561105],"translation_xyz_m":[0.06405760253824525,0.05206923170331782,1.0280057719733866],"reprojection_error_px":0.17092226720822715},{"quaternion_xyzw":[-0.2490776805659148,0.08059931165633491,0.8208033663612531,-0.5076868067760514],"translation_xyz_m":[0.06410524749481918,0.052079512501452846,1.0278771241968907],"reprojection_error_px":0.3835583284508621}]}]},{"stamp_ns":1789379975006467773,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09371761667993744}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2221984612080657,0.3489129103253785,-0.4323934205381253,0.8012013197195708],"translation_xyz_m":[0.060038978567753004,0.09355020366543741,1.0232499317166799],"reprojection_error_px":0.34377074499466553},{"quaternion_xyzw":[0.1229204719699535,-0.3393791826497924,-0.4111538970594752,0.8370572267697408],"translation_xyz_m":[0.0600988284787855,0.09354595092180938,1.0232088737771157],"reprojection_error_px":0.40176838983446345}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.325230244814039,-0.07054722755164436,0.8066634550627256,-0.48840807406229764],"translation_xyz_m":[0.06403034632007855,0.05204382250939118,1.0277419685999107],"reprojection_error_px":0.16794062207054938},{"quaternion_xyzw":[-0.24947521531817318,0.0807275702594144,0.8207008534794068,-0.5076369622477619],"translation_xyz_m":[0.064078158498087,0.05205420770632255,1.027615152792864],"reprojection_error_px":0.3800316114335088}]}]},{"stamp_ns":1789379975039804549,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10735983437968966}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22239314358285664,0.34887438196780357,-0.4323192425696295,0.801204111196354],"translation_xyz_m":[0.060045879720376526,0.09358373737324735,1.023588777715605],"reprojection_error_px":0.34709626392374415},{"quaternion_xyzw":[0.12312169704198135,-0.3393439153566701,-0.41109212881103463,0.8370722886689772],"translation_xyz_m":[0.06010585574867077,0.09357974814347306,1.0235502102130156],"reprojection_error_px":0.40198186359579463}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3250568419573465,-0.07058203203274718,0.8067342569097767,-0.4884015407214389],"translation_xyz_m":[0.06403099426038272,0.05205774858887082,1.027961139463756],"reprojection_error_px":0.16677044202904517},{"quaternion_xyzw":[-0.24930681547958322,0.08077489573538009,0.8207551760665915,-0.5076243383982549],"translation_xyz_m":[0.0640788359958326,0.052068146998211585,1.0278350992279375],"reprojection_error_px":0.3784857309105992}]}]},{"stamp_ns":1789379975073207968,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09714796977606076}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22226220809894875,0.34876330934324035,-0.43238882009387064,0.8012512547040169],"translation_xyz_m":[0.06003979048608452,0.09359781136594648,1.0235954950699773],"reprojection_error_px":0.34460228462697684},{"quaternion_xyzw":[0.12297576324412368,-0.33924928168232743,-0.41115686350531666,0.8371003046977228],"translation_xyz_m":[0.06009953383200353,0.09359344336821736,1.0235529600441964],"reprojection_error_px":0.4041228476485205}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32472361639083125,-0.07047189167423218,0.806912370051042,-0.4883448704558346],"translation_xyz_m":[0.06403742476552485,0.05207958157338317,1.0281385912437957],"reprojection_error_px":0.1642861977761874},{"quaternion_xyzw":[-0.2489639318492531,0.08068859528797342,0.820915289327761,-0.5075474351984576],"translation_xyz_m":[0.0640852721986608,0.05208999512075115,1.0280132585438315],"reprojection_error_px":0.37644914238765276}]}]},{"stamp_ns":1789379975106550198,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.043839694552571216}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22235033617409963,0.3487040408907664,-0.43235134896348276,0.8012728192814057],"translation_xyz_m":[0.060049915594810765,0.09359549896305006,1.02348665618748],"reprojection_error_px":0.3483466949390823},{"quaternion_xyzw":[0.12305104912091988,-0.33917458615147306,-0.41113096096961443,0.8371322311029599],"translation_xyz_m":[0.06010972529575083,0.09359124437045431,1.0234451510880587],"reprojection_error_px":0.40637287083912144}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3245662333181261,-0.07039717448083863,0.8068753794907962,-0.4885213608291674],"translation_xyz_m":[0.06405323036015485,0.052086871831046315,1.0280991987185997],"reprojection_error_px":0.1666892283395395},{"quaternion_xyzw":[-0.24878211234418993,0.08059970136049563,0.8208761501841607,-0.5077139891484113],"translation_xyz_m":[0.06410098437952036,0.05209723781191011,1.0279727099506757],"reprojection_error_px":0.3790025438282079}]}]},{"stamp_ns":1789379975140107828,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0427532742046781}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.222309486987812,0.3487650111754906,-0.43233252694287144,0.8012677736700555],"translation_xyz_m":[0.060058429698924054,0.09358322167273204,1.023383970481262],"reprojection_error_px":0.34378983375376354},{"quaternion_xyzw":[0.12300727925506615,-0.339221953486353,-0.4111076409543632,0.8371309234953079],"translation_xyz_m":[0.060118204821511353,0.09357889517273427,1.023341776344094],"reprojection_error_px":0.40304756169931927}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3247901131739643,-0.07012402057805961,0.8068157619445125,-0.4885103175986227],"translation_xyz_m":[0.06405593089049798,0.05207614501779267,1.0279372645029727],"reprojection_error_px":0.16799201829868385},{"quaternion_xyzw":[-0.2490015758192987,0.08031584730923115,0.8208481903857233,-0.507696590741941],"translation_xyz_m":[0.06410361106006875,0.052086518056888055,1.0278095627789507],"reprojection_error_px":0.38108752168808724}]}]},{"stamp_ns":1789379975173379114,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04084496684637998}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22228932057884646,0.3486654739510932,-0.43236895504964634,0.8012970310317711],"translation_xyz_m":[0.06005359144958395,0.09357688846206383,1.0234082870931176],"reprojection_error_px":0.3505511163746353},{"quaternion_xyzw":[0.12299332313108609,-0.33912538398813397,-0.41115447024574386,0.8371491014132516],"translation_xyz_m":[0.060113356248500845,0.09357255158792589,1.0233659425459602],"reprojection_error_px":0.4092765424591054}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3243470542775729,-0.07016649408276626,0.8069830833604597,-0.48852221511374444],"translation_xyz_m":[0.06405899290476333,0.052074064058046446,1.0280932220851502],"reprojection_error_px":0.17109493934401465},{"quaternion_xyzw":[-0.24855914091952963,0.08036287212025649,0.8209842451561945,-0.5076859575123606],"translation_xyz_m":[0.06410659788178505,0.05208435788939591,1.0279648714399157],"reprojection_error_px":0.3833279880821381}]}]},{"stamp_ns":1789379975206876726,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0661595691278275}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22225192957880707,0.34870046723382886,-0.43233221430852187,0.8013119994237555],"translation_xyz_m":[0.06003960229750314,0.09357003437065387,1.0233528608463118],"reprojection_error_px":0.34851698382653223},{"quaternion_xyzw":[0.1229622878629675,-0.3391637872851779,-0.41111102265222427,0.8371594401377324],"translation_xyz_m":[0.060099426529028256,0.09356577905766808,1.023311461558236],"reprojection_error_px":0.40640790548338207}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3244033061129761,-0.0703564035553177,0.8069603160472575,-0.48849515840655194],"translation_xyz_m":[0.06404868902992947,0.05207238359009967,1.0280895661971037],"reprojection_error_px":0.17221540694423226},{"quaternion_xyzw":[-0.24862713668885922,0.08055623694293967,0.8209529062745248,-0.5076726950220032],"translation_xyz_m":[0.06409627357496966,0.05208261513801753,1.0279605533171203],"reprojection_error_px":0.38463800862355335}]}]},{"stamp_ns":1789379975239987221,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09103597054033893}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22234703612013784,0.3487239061985334,-0.4323664400276369,0.8012569464984716],"translation_xyz_m":[0.06001282969016152,0.0935360715179583,1.0230060461314385],"reprojection_error_px":0.34891470500889504},{"quaternion_xyzw":[0.1230648185343179,-0.33919815305050344,-0.4111480096605328,0.8371122849166801],"translation_xyz_m":[0.06007257662790647,0.09353167125663998,1.0229629692757178],"reprojection_error_px":0.40867855897698757}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3241880778582345,-0.07018920485893457,0.807068099105982,-0.48848403157248715],"translation_xyz_m":[0.06404847141932374,0.05207713299670082,1.0282121961609272],"reprojection_error_px":0.1735092424315464},{"quaternion_xyzw":[-0.24841280340333932,0.08040007691185984,0.821053181227349,-0.5076402075627096],"translation_xyz_m":[0.06409604674830122,0.0520873897103123,1.0280835571744464],"reprojection_error_px":0.384790828454624}]}]},{"stamp_ns":1789379975273397746,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08527736491133404}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22223737510697286,0.348698962514624,-0.4323191823812469,0.8013237218452962],"translation_xyz_m":[0.06001091368515086,0.09354357794631905,1.0230073641687285],"reprojection_error_px":0.3407707542784452},{"quaternion_xyzw":[0.12294593962601645,-0.3391709884059585,-0.41109276936500927,0.8371678873015905],"translation_xyz_m":[0.06007063122715303,0.09353912038252014,1.0229637720221216],"reprojection_error_px":0.40195097199900975}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3240330916119544,-0.07012362946874932,0.8071015104428737,-0.4885410770563314],"translation_xyz_m":[0.06405399443771412,0.052089490717507084,1.0283256115090986],"reprojection_error_px":0.1689881922710573},{"quaternion_xyzw":[-0.2482509912922935,0.08033718436612292,0.8210795191370794,-0.5076867197240238],"translation_xyz_m":[0.06410163933029195,0.05209982114296272,1.0281984579445351],"reprojection_error_px":0.38085750168495275}]}]},{"stamp_ns":1789379975306826916,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05901310869343591}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222998797809885,0.3486864990468966,-0.43233274346109085,0.8013044912910343],"translation_xyz_m":[0.06002771619725765,0.09355046357256407,1.0231087168561224],"reprojection_error_px":0.34675464898843916},{"quaternion_xyzw":[0.1230070697138013,-0.3391498137431689,-0.4111148441426608,0.8371566457742264],"translation_xyz_m":[0.0600875413481156,0.09354619355632526,1.0230670780288706],"reprojection_error_px":0.4051319366888803}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3240164464645645,-0.07013773999068727,0.8071170127868043,-0.4885244799593776],"translation_xyz_m":[0.06405828020510493,0.052085441705989305,1.0282475268229814],"reprojection_error_px":0.17172942812632366},{"quaternion_xyzw":[-0.24822742981491863,0.08034927963675752,0.8210954175015525,-0.507670613397393],"translation_xyz_m":[0.06410586535353825,0.05209571729180121,1.028119364579931],"reprojection_error_px":0.38339098470856986}]}]},{"stamp_ns":1789379975340058865,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04403449276517272}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222515516440261,0.3487591960522825,-0.43227717962473566,0.8013162365361582],"translation_xyz_m":[0.06002993804844011,0.09353287498148963,1.022945254330679],"reprojection_error_px":0.33913304142618605},{"quaternion_xyzw":[0.12295664548009028,-0.3392069579912281,-0.41105258588969573,0.8371714726482353],"translation_xyz_m":[0.06008977424241816,0.09352859854072626,1.022903633201626],"reprojection_error_px":0.39828710467301415}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32402896321268565,-0.07030004953560169,0.8070768823594221,-0.4885591468753802],"translation_xyz_m":[0.06405815122268808,0.05207309589815945,1.0280532063508692],"reprojection_error_px":0.17636120973518712},{"quaternion_xyzw":[-0.24823348493965405,0.08049948730694832,0.8210511579897295,-0.5077154374861543],"translation_xyz_m":[0.06410562158804177,0.05208324136202955,1.0279228654148234],"reprojection_error_px":0.38825989589448306}]}]},{"stamp_ns":1789379975373426109,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0445663938517521}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22214961804811204,0.34880894311152694,-0.43226403531701985,0.801329939649046],"translation_xyz_m":[0.06002399845542116,0.09352992482595764,1.0228719038722769],"reprojection_error_px":0.33621947067160074},{"quaternion_xyzw":[0.12285191744058165,-0.339257561362991,-0.4110270003843203,0.8371789046519728],"translation_xyz_m":[0.060083851437581395,0.09352564999857757,1.0228304906996655],"reprojection_error_px":0.39543518094156527}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32388686606444356,-0.07023871200550956,0.8071594801927091,-0.4885257361304477],"translation_xyz_m":[0.0640605032403639,0.052079868906708196,1.0281208700730948],"reprojection_error_px":0.1755824581986847},{"quaternion_xyzw":[-0.24808726195478692,0.0804493747387279,0.8211267700492948,-0.5076725677721196],"translation_xyz_m":[0.0641080290855859,0.052090066564000476,1.027991694107794],"reprojection_error_px":0.38650736546864745}]}]},{"stamp_ns":1789379975406705696,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04719489062983438}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2223592025989245,0.34863300849276047,-0.4323017571627031,0.8013280234478866],"translation_xyz_m":[0.060013530127932965,0.09353076777175541,1.022869013373977],"reprojection_error_px":0.3418714398092918},{"quaternion_xyzw":[0.12306361536837063,-0.33909504198911283,-0.41109074999021805,0.8371823542952559],"translation_xyz_m":[0.06007320794344408,0.09352625902764604,1.022824605377102],"reprojection_error_px":0.4038805357745653}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32392398191999866,-0.07030302067063636,0.8071250511201516,-0.48854876018261384],"translation_xyz_m":[0.06404559044753082,0.05207832935295546,1.0280470624640619],"reprojection_error_px":0.1718874202970517},{"quaternion_xyzw":[-0.2481317624700476,0.08051621677495009,0.8210888398255571,-0.5077015702198269],"translation_xyz_m":[0.06409321637691909,0.05208859309614384,1.0279192925732568],"reprojection_error_px":0.38310655236003405}]}]},{"stamp_ns":1789379975439957273,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06434068981762038}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22225576221221383,0.3486540441044705,-0.4323304709275459,0.801332076982216],"translation_xyz_m":[0.06000579176393939,0.09353618479002342,1.0228813982879885],"reprojection_error_px":0.34125346851811017},{"quaternion_xyzw":[0.1229583430982492,-0.33912608205627975,-0.4111081984663873,0.8371766811643837],"translation_xyz_m":[0.06006559988777567,0.09353186105305487,1.0228392177781107],"reprojection_error_px":0.4008302988012733}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3236184475205105,-0.0701097856479904,0.8072173873142515,-0.4886264503671094],"translation_xyz_m":[0.0640536283549243,0.05209714483138983,1.0283184112204522],"reprojection_error_px":0.17112395671066405},{"quaternion_xyzw":[-0.24782057824100043,0.08033368834606834,0.821168554435539,-0.5077535472286338],"translation_xyz_m":[0.06410132010048053,0.05210750902094325,1.0281925171476438],"reprojection_error_px":0.3804218076267403}]}]},{"stamp_ns":1789379975473283255,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0761354367783128}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22228814267284866,0.3487391607346116,-0.4322894064560221,0.8013082106549728],"translation_xyz_m":[0.05999823230113809,0.09352170150090962,1.0227753621388613],"reprojection_error_px":0.3401901165282225},{"quaternion_xyzw":[0.12300022199890628,-0.33920927935488726,-0.4110626164050562,0.837159205636531],"translation_xyz_m":[0.06005779007332318,0.09351697942064777,1.0227288101920389],"reprojection_error_px":0.40476060465497565}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32372659619203176,-0.07007610961066135,0.8071820461745512,-0.48861802475269],"translation_xyz_m":[0.06405446958018095,0.05209377452541607,1.0283522393165248],"reprojection_error_px":0.17679786378430823},{"quaternion_xyzw":[-0.24793674367889945,0.08029467686435944,0.8211417516383437,-0.5077463537214864],"translation_xyz_m":[0.064101946100296,0.05210397586768147,1.0282228266509414],"reprojection_error_px":0.38729272682773724}]}]},{"stamp_ns":1789379975506650810,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09348540077491492}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2223029532245338,0.348656915699294,-0.4323146535469529,0.8013262709104727],"translation_xyz_m":[0.06000282696786107,0.09350468904408928,1.0227592361723106],"reprojection_error_px":0.3349385928223354},{"quaternion_xyzw":[0.1230203868446497,-0.3391185650008396,-0.4111024500867399,0.8371734341371827],"translation_xyz_m":[0.0600624516915768,0.0935000765771276,1.0227137964025224],"reprojection_error_px":0.3989018394174674}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32342781889359146,-0.07000018557456475,0.8072260684998642,-0.4887540223052011],"translation_xyz_m":[0.06406893351007009,0.05208595537514182,1.0284925808356298],"reprojection_error_px":0.1685649248727419},{"quaternion_xyzw":[-0.24763117380925012,0.08019585846327494,0.8211748497570125,-0.507857551061062],"translation_xyz_m":[0.06411654609690695,0.05209629613682258,1.0283659738865147],"reprojection_error_px":0.3800295181069422}]}]},{"stamp_ns":1789379975539938727,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11594812208450915}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222193374933706,0.348740721529012,-0.4323181773774331,0.8013110935838127],"translation_xyz_m":[0.06001484882083475,0.093494297165924,1.02273337929738],"reprojection_error_px":0.33815006049167207},{"quaternion_xyzw":[0.12293931059296387,-0.3391881748562696,-0.41109955856348374,0.8371585637724244],"translation_xyz_m":[0.06007456701960685,0.09348981198100995,1.0226895242707308],"reprojection_error_px":0.3999507774643976}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32373174590649584,-0.07015616261742211,0.8070955977461115,-0.48874591112149124],"translation_xyz_m":[0.06406466245853475,0.052062005787404504,1.028204182424921],"reprojection_error_px":0.16788615248085373},{"quaternion_xyzw":[-0.24793591250315516,0.08033166213049206,0.8210604164087518,-0.5078724248837175],"translation_xyz_m":[0.06411234178338272,0.052072370026174264,1.0280779048457573],"reprojection_error_px":0.37939625218593204}]}]},{"stamp_ns":1789379975573584248,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08405536508509358}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2219479366716688,0.3488845529043296,-0.432162028984718,0.8014079253761761],"translation_xyz_m":[0.06002194639059016,0.0935059595309307,1.0227262719090497],"reprojection_error_px":0.3295112707519519},{"quaternion_xyzw":[0.12265413339196775,-0.3393099769029054,-0.41091153878138087,0.8372433400346239],"translation_xyz_m":[0.060081907911413567,0.09350181479599302,1.0226865517126005],"reprojection_error_px":0.3874683666169154}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3237926561788732,-0.07024032982321013,0.8071175522373256,-0.4886572098530673],"translation_xyz_m":[0.0640739439232395,0.052073716588266636,1.0282271411452906],"reprojection_error_px":0.1751988652231598},{"quaternion_xyzw":[-0.2479897699714462,0.08042782056358595,0.8210822118287769,-0.5077956686377546],"translation_xyz_m":[0.06412144447831579,0.05208390912926197,1.0280978249634125],"reprojection_error_px":0.38645973041641773}]}]},{"stamp_ns":1789379975606821824,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03557917871825869}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222726070796013,0.34865794261459065,-0.43227443350825306,0.8013559392235868],"translation_xyz_m":[0.06001248276802652,0.09352689079865917,1.0227573264058087],"reprojection_error_px":0.33912678882742436},{"quaternion_xyzw":[0.12296839534325225,-0.3391137877749218,-0.41105472207940935,0.8372064429660527],"translation_xyz_m":[0.06007223620911031,0.09352247690122659,1.0227140916285569],"reprojection_error_px":0.4001265524998553}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3236593191507224,-0.07012374867385207,0.8072295587259363,-0.48857726565810194],"translation_xyz_m":[0.06406832763002761,0.05209921068169156,1.0283170650795312],"reprojection_error_px":0.1759890975534784},{"quaternion_xyzw":[-0.2478524034380135,0.08034478907062113,0.8211860707452546,-0.5077079260688839],"translation_xyz_m":[0.06411582080859797,0.052109408476354944,1.0281879340546964],"reprojection_error_px":0.3865986214935906}]}]},{"stamp_ns":1789379975640245192,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09997468519512084}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22238674597890892,0.3486618811905571,-0.43231048729151095,0.8013031076910601],"translation_xyz_m":[0.05998398991240003,0.09352481069388881,1.0226170373805727],"reprojection_error_px":0.3428103138645756},{"quaternion_xyzw":[0.12308567679193282,-0.3391441769665032,-0.411089292894321,0.8371599229932309],"translation_xyz_m":[0.06004370271377598,0.09352033100460504,1.0225729584656271],"reprojection_error_px":0.40439230551701955}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3234286667084191,-0.07003233017905817,0.8072962428740261,-0.4886329363872099],"translation_xyz_m":[0.06405156205878548,0.05211110554973699,1.0283698166126436],"reprojection_error_px":0.17229699656339664},{"quaternion_xyzw":[-0.24762587201266834,0.08027047692590919,0.8212367321427663,-0.5077482721029418],"translation_xyz_m":[0.064099159882883,0.052121403936590344,1.0282427488277246],"reprojection_error_px":0.38239683986975176}]}]},{"stamp_ns":1789379975673488216,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09951461146811087}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222096158842558,0.34866906291411426,-0.4322918432365811,0.801359178799547],"translation_xyz_m":[0.059979875079763434,0.09350554129756626,1.02260107649297],"reprojection_error_px":0.34459950177104426},{"quaternion_xyzw":[0.1229210855186181,-0.3391412774298036,-0.411066125112452,0.8371966563856066],"translation_xyz_m":[0.060039668362854785,0.09350116388631839,1.0225583513023215],"reprojection_error_px":0.40445455802400015}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32339749031453835,-0.06994654366944572,0.8072955865879778,-0.4886669419579447],"translation_xyz_m":[0.06405934360632166,0.05210272935917221,1.0285442129737012],"reprojection_error_px":0.17280546343858916},{"quaternion_xyzw":[-0.24760470295033893,0.08016993644589908,0.8212396250117053,-0.5077698008721753],"translation_xyz_m":[0.06410687159498786,0.05211300085973442,1.028416319124353],"reprojection_error_px":0.38355182509598257}]}]},{"stamp_ns":1789379975706757993,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1105108066088627}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22220567695323815,0.34868509955005655,-0.43229369456437267,0.8013522946378859],"translation_xyz_m":[0.060001997499854375,0.09349515859812436,1.0227166787972226],"reprojection_error_px":0.34294505096807054},{"quaternion_xyzw":[0.1229273930323085,-0.33913767913419707,-0.4110768940451326,0.8371919002331292],"translation_xyz_m":[0.06006176984091353,0.09349076023167727,1.0226737237484116],"reprojection_error_px":0.4032151885723578}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3239822147889721,-0.07014051254270827,0.8070844143251603,-0.4886006356464258],"translation_xyz_m":[0.06405184386063824,0.05206250958829601,1.0281930377134538],"reprojection_error_px":0.17069723964831435},{"quaternion_xyzw":[-0.2482031968814767,0.08033177931415013,0.8210620397002857,-0.5077392098836215],"translation_xyz_m":[0.06409947336096584,0.05207282892275879,1.028065600215035],"reprojection_error_px":0.3820702120823802}]}]},{"stamp_ns":1789379975740311678,"direction":"decreasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.14052423483648366}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22228476101288608,0.34877913544266914,-0.43237506721590757,0.8012455310024187],"translation_xyz_m":[0.060019438576232806,0.09348083390551573,1.0226717027651158],"reprojection_error_px":0.3432754554787904},{"quaternion_xyzw":[0.12301081145710459,-0.3392251584048099,-0.4111615227261282,0.8371026426905053],"translation_xyz_m":[0.06007913675447186,0.09347631542366311,1.022627459794365],"reprojection_error_px":0.4049645563871903}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3238814138006566,-0.07018213265068778,0.8070579473391147,-0.4887051950694443],"translation_xyz_m":[0.06406811429399556,0.052052099005954824,1.0281553604449396],"reprojection_error_px":0.17981531703818004},{"quaternion_xyzw":[-0.24809118779133313,0.08034959681249157,0.821032718662464,-0.5078385370549174],"translation_xyz_m":[0.0641154474189371,0.05206217789616205,1.028023278875639],"reprojection_error_px":0.3919349510746901}]}]},{"stamp_ns":1789379976873892815,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09126755385963639}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22228384734939474,0.3489106870729436,-0.4324214893692479,0.801163453475955],"translation_xyz_m":[0.060050120321292415,0.09360911269366948,1.023753931432851],"reprojection_error_px":0.34361563204215445},{"quaternion_xyzw":[0.1230053655157788,-0.33940218636884134,-0.41117845704751504,0.8370233643109402],"translation_xyz_m":[0.06010981652540971,0.09360467598767273,1.0237107674090824],"reprojection_error_px":0.4039168374697499}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3254244070711275,-0.0704065719198545,0.8067210771496172,-0.48820382382306354],"translation_xyz_m":[0.0640160820228446,0.052062678270479716,1.0278388414199247],"reprojection_error_px":0.16236897668701264},{"quaternion_xyzw":[-0.24968323585853433,0.08062357845469142,0.8207708107150197,-0.5074380716960257],"translation_xyz_m":[0.06406407327170648,0.05207322533103699,1.0277148121000776],"reprojection_error_px":0.3739964783058767}]}]},{"stamp_ns":1789379976907236493,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06252820479156768}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222614727432251,0.34873090549410235,-0.4324248105936413,0.8012461397537771],"translation_xyz_m":[0.060065699248479536,0.09362631625321417,1.023889057723222],"reprojection_error_px":0.3494390618711628},{"quaternion_xyzw":[0.12296919404434425,-0.3392133872542647,-0.41119720261606085,0.8370960015340528],"translation_xyz_m":[0.06012547512995516,0.09362202757525795,1.0238473932754975],"reprojection_error_px":0.4074653895199829}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3251146480304826,-0.07031531098228391,0.8068120690672633,-0.48827298500416927],"translation_xyz_m":[0.06403192387672196,0.052075523213603946,1.0279827102804284],"reprojection_error_px":0.16510250965435525},{"quaternion_xyzw":[-0.24935810134126737,0.08053370510250005,0.8208480161629891,-0.5074873338313252],"translation_xyz_m":[0.06407979000982111,0.05208598952631753,1.0278572867020384],"reprojection_error_px":0.37693937075236783}]}]},{"stamp_ns":1789379976940940943,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03390026839520768}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.222294170805021,0.34867174269894025,-0.432447544917449,0.8012505465609762],"translation_xyz_m":[0.06007596002769634,0.09362077385583592,1.0238269289297237],"reprojection_error_px":0.35055840659899995},{"quaternion_xyzw":[0.1229945301702684,-0.3391457557655828,-0.4112297184371527,0.837103709565599],"translation_xyz_m":[0.060135643712479585,0.09361634386421532,1.0237836382974168],"reprojection_error_px":0.4102649212954336}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32500158653375766,-0.07036589146435662,0.8068024551112317,-0.48835684544752284],"translation_xyz_m":[0.06404405654512595,0.05207602374959763,1.0279354398332923],"reprojection_error_px":0.16655443362112982},{"quaternion_xyzw":[-0.24922906684966492,0.08057295255237171,0.8208325563073802,-0.5075694888979537],"translation_xyz_m":[0.06409185654917485,0.052086429214270986,1.0278090470130543],"reprojection_error_px":0.3788237283949338}]}]},{"stamp_ns":1789379976974054837,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.043543421018917346}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22227263875472816,0.3488220985361177,-0.4324306297695201,0.8012002047372462],"translation_xyz_m":[0.060067440171267,0.0935943777296238,1.023600282578896],"reprojection_error_px":0.34962425155575005},{"quaternion_xyzw":[0.12298060784685795,-0.33929025516047107,-0.4112010353440333,0.8370612889022851],"translation_xyz_m":[0.06012721829822038,0.09359006103264271,1.0235583759583398],"reprojection_error_px":0.40793562776749903}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32508216401593676,-0.0704992622015785,0.8068002008440285,-0.4882876985812606],"translation_xyz_m":[0.06404534269815067,0.05206531331781997,1.027888220002112],"reprojection_error_px":0.17062444614590466},{"quaternion_xyzw":[-0.24931437986741592,0.08070239645158472,0.8208302686765937,-0.5075107222735378],"translation_xyz_m":[0.06409305924773086,0.052075617297940476,1.027760180519759],"reprojection_error_px":0.382711390766391}]}]},{"stamp_ns":1789379977007232782,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06118173675388661}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22212764845803795,0.3486751612469666,-0.4324280366585001,0.8013057673769601],"translation_xyz_m":[0.06007524507034557,0.09360565599869561,1.0237726484959568],"reprojection_error_px":0.3470980818295917},{"quaternion_xyzw":[0.12283144867899054,-0.33913891477808633,-0.41120413651874144,0.837142992450178],"translation_xyz_m":[0.06013490912106468,0.0936011692848486,1.0237289631737057],"reprojection_error_px":0.4075739651697254}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3250498936134821,-0.07029923987155345,0.8067909705555538,-0.4883532669752226],"translation_xyz_m":[0.06406036790449982,0.0520752213341333,1.0281745590283429],"reprojection_error_px":0.16486229487009477},{"quaternion_xyzw":[-0.24928271400337973,0.08049552510518801,0.8208293217266415,-0.5075606599520209],"translation_xyz_m":[0.06410820780891581,0.052085685292429365,1.0280490479040694],"reprojection_error_px":0.37686074540129616}]}]},{"stamp_ns":1789379977040622599,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06942313231039501}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22252083621639412,0.34877597034542424,-0.43236431294613625,0.8011871821551941],"translation_xyz_m":[0.06006439636629845,0.09360439735872354,1.0237694175406493],"reprojection_error_px":0.34077726436259836},{"quaternion_xyzw":[0.12324084345958107,-0.33924922128354684,-0.4111521952439739,0.8370636371913066],"translation_xyz_m":[0.060124041475593165,0.09359993031845736,1.023725392172618],"reprojection_error_px":0.40238581627196873}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.325159409576519,-0.07042495961451692,0.8067624725908952,-0.4883093243489912],"translation_xyz_m":[0.06404010843220286,0.052066190442655536,1.0280450660985119],"reprojection_error_px":0.1588759532127055},{"quaternion_xyzw":[-0.24940553365846602,0.08062521180191917,0.8207988282282223,-0.5075290519582192],"translation_xyz_m":[0.06408813010403336,0.05207676811035282,1.0279220438444057],"reprojection_error_px":0.3711214755295527}]}]},{"stamp_ns":1789379977073958925,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09573102329367057}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222647380733758,0.3487215507421172,-0.43236210720995955,0.8012831425321201],"translation_xyz_m":[0.06006150511878219,0.09363012900758647,1.0239683852515093],"reprojection_error_px":0.3409916845651376},{"quaternion_xyzw":[0.12297768400320386,-0.3392034518442101,-0.41113446694417305,0.8371295942592829],"translation_xyz_m":[0.0601212205199303,0.09362575802454165,1.023925750912882],"reprojection_error_px":0.4009528840891605}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3250901879514327,-0.07046487291278959,0.8068216680222506,-0.4882518483251032],"translation_xyz_m":[0.06403332275155761,0.0520771792379182,1.0281196630098743],"reprojection_error_px":0.16212721492843066},{"quaternion_xyzw":[-0.24934132488614885,0.08068426135449952,0.8208472996334868,-0.5074728212999309],"translation_xyz_m":[0.06408129929549335,0.05208770206179795,1.0279959540817623],"reprojection_error_px":0.3734278960481306}]}]},{"stamp_ns":1789379977107285300,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07825176270770275}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2223128947511881,0.3488090842305851,-0.43242230298385315,0.801199195873104],"translation_xyz_m":[0.060046102064194785,0.09360942237857846,1.0236257582978083],"reprojection_error_px":0.34257265053980435},{"quaternion_xyzw":[0.12301936329575341,-0.33930061313688287,-0.41118635852274377,0.8370586053222607],"translation_xyz_m":[0.06010590047945508,0.09360514297126944,1.023584196660841],"reprojection_error_px":0.40123131844550763}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3251657516597718,-0.07074476868033844,0.8065119956814744,-0.4886725002225788],"translation_xyz_m":[0.06404101824868771,0.052066600069803966,1.027807174072363],"reprojection_error_px":0.154282404002902},{"quaternion_xyzw":[-0.24938971933826315,0.08090996031905996,0.8205366254936632,-0.507915339828423],"translation_xyz_m":[0.06408917356799808,0.052077241231475654,1.0276858822776358],"reprojection_error_px":0.36702707433275866}]}]},{"stamp_ns":1789379977140705412,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03889087980617529}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22244019677373794,0.3486006937796997,-0.4323932112854905,0.8012702577718667],"translation_xyz_m":[0.060089519800454017,0.09365000051468886,1.0240703784034015],"reprojection_error_px":0.33920144386665807},{"quaternion_xyzw":[0.12313304694680363,-0.3390731231915245,-0.4111868142669213,0.8371338445265261],"translation_xyz_m":[0.06014930635814178,0.09364578050847401,1.024029065920043],"reprojection_error_px":0.39789936250437136}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3254471954729098,-0.07042716909533905,0.8061883004826902,-0.48906498645565094],"translation_xyz_m":[0.06408438365631444,0.05206980466626245,1.028022708703741],"reprojection_error_px":0.16410187838524948},{"quaternion_xyzw":[-0.24965382685090987,0.08053119723010502,0.8202559493126561,-0.5082989972728408],"translation_xyz_m":[0.06413217615514268,0.05208028248505436,1.0278960847938867],"reprojection_error_px":0.37785171022897784}]}]},{"stamp_ns":1789379977173801980,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04829956409795532}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22240605210868494,0.3486113735521717,-0.43237387076310274,0.8012855259499131],"translation_xyz_m":[0.06010273662900513,0.09364671795427816,1.0241274580710935],"reprojection_error_px":0.3496014408737716},{"quaternion_xyzw":[0.12310412957653165,-0.3390694577352722,-0.41117067360794557,0.8371475098673478],"translation_xyz_m":[0.06016236203881591,0.09364225200952572,1.0240834851002967],"reprojection_error_px":0.41012414377206025}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32564143054912104,-0.07059861078783099,0.8061398630319457,-0.4889908139173845],"translation_xyz_m":[0.06408141894597714,0.05205033250293962,1.0277874503745],"reprojection_error_px":0.165905182507362},{"quaternion_xyzw":[-0.24984794277248554,0.08069487501963718,0.8202145862425735,-0.5082444049400305],"translation_xyz_m":[0.06412924454986425,0.0520607939668428,1.027660739796241],"reprojection_error_px":0.37887835988741775}]}]},{"stamp_ns":1789379977207316768,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.061488571106748255}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22237517783627497,0.3487168781125182,-0.4323713605339518,0.8012495402755395],"translation_xyz_m":[0.060081536179179995,0.0936100737918022,1.0238293584534055],"reprojection_error_px":0.3404235865848199},{"quaternion_xyzw":[0.12308290501230727,-0.339175220009876,-0.41115888984495785,0.837113574090425],"translation_xyz_m":[0.060141436898692786,0.09360599490240301,1.0237897513911631],"reprojection_error_px":0.397093495849323}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32542379648541253,-0.07060372367147086,0.8061702369671864,-0.4890848759806565],"translation_xyz_m":[0.06407863230718953,0.05204018422479961,1.0277901335791007],"reprojection_error_px":0.16452108479474978},{"quaternion_xyzw":[-0.24963136884342574,0.08068901121663749,0.820231501752777,-0.5083244502191715],"translation_xyz_m":[0.06412648742745908,0.052050671894527924,1.0276641596661877],"reprojection_error_px":0.37738563109256207}]}]},{"stamp_ns":1789379977240493573,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07408445277041434}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22232591705417704,0.3489173209197498,-0.4322492931643879,0.8012418101466563],"translation_xyz_m":[0.060074866906023414,0.09361360961776634,1.023862287630035],"reprojection_error_px":0.3394019673801619},{"quaternion_xyzw":[0.12304461796110623,-0.33937081859080825,-0.41101453432832624,0.837110818261644],"translation_xyz_m":[0.06013481225679717,0.09360959077723664,1.0238234789140404],"reprojection_error_px":0.395238535502028}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32486624273338494,-0.07121858162181928,0.805778629910724,-0.49001085451576076],"translation_xyz_m":[0.06411067283882546,0.052022106699839976,1.0278152367964388],"reprojection_error_px":0.16354400029980876},{"quaternion_xyzw":[-0.24903837173146987,0.08119435227391035,0.8197858516531045,-0.5092529076927814],"translation_xyz_m":[0.06415852956585946,0.052032533378786355,1.027689621273882],"reprojection_error_px":0.376604108809261}]}]},{"stamp_ns":1789379977274266841,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09670324830885667}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22222923140102244,0.348932859800277,-0.43217234688209194,0.8013033699248593],"translation_xyz_m":[0.06007477208932579,0.09364660636213136,1.0240827387396256],"reprojection_error_px":0.3448564882215346},{"quaternion_xyzw":[0.12294462054809698,-0.3393913403088333,-0.4109240634160987,0.8371616047736902],"translation_xyz_m":[0.06013454530126119,0.09364233153588662,1.0240412293530343],"reprojection_error_px":0.40310975644326225}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3244140565611953,-0.07134616247306803,0.8055212955742106,-0.49071446624521015],"translation_xyz_m":[0.06414855943518914,0.05204212284690307,1.028136605200736],"reprojection_error_px":0.16289472221163442},{"quaternion_xyzw":[-0.24855276060493492,0.08125752133548385,0.8194994289478709,-0.5099406106363691],"translation_xyz_m":[0.06419634646271366,0.05205252633537237,1.0280108118460138],"reprojection_error_px":0.37644836572900725}]}]},{"stamp_ns":1789379977307336145,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10395211376306472}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22237007255664765,0.3488099338686491,-0.4321558835400456,0.8013266956662697],"translation_xyz_m":[0.06007144283557226,0.09365563499791366,1.0240612504452153],"reprojection_error_px":0.34667201195936675},{"quaternion_xyzw":[0.12307582537548839,-0.339271558653266,-0.410922850424337,0.8371914725415349],"translation_xyz_m":[0.06013127260112969,0.0936514750611589,1.0240206750972034],"reprojection_error_px":0.40374085524219494}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32434234926549105,-0.07168257895385466,0.8052259856986033,-0.49119727228800775],"translation_xyz_m":[0.06415914295921286,0.052033813318671025,1.0279312159613474],"reprojection_error_px":0.16091002630081966},{"quaternion_xyzw":[-0.24845564690020078,0.08153712307116541,0.8191899141828327,-0.5104403722138401],"translation_xyz_m":[0.06420698050299446,0.05204422169564044,1.0278059772302623],"reprojection_error_px":0.3749654290924389}]}]},{"stamp_ns":1789379977340704138,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03359126573259876}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2222569768799381,0.34915732121355414,-0.43159079650877846,0.8015113134822763],"translation_xyz_m":[0.060091445586074155,0.09364579279012805,1.0238447167264673],"reprojection_error_px":0.3377216173284984},{"quaternion_xyzw":[0.12295451760913788,-0.3395319129422939,-0.4103248250578248,0.8373970412143312],"translation_xyz_m":[0.06015162319205146,0.0936421570197057,1.0238098575575885],"reprojection_error_px":0.3891126726947642}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32437444692621653,-0.07261000308786528,0.8043019490822488,-0.4925519062346493],"translation_xyz_m":[0.06424740587634738,0.05200431926189166,1.027590981803008],"reprojection_error_px":0.15820561548178866},{"quaternion_xyzw":[-0.24839307497890642,0.08227172451611846,0.8182540564447913,-0.5118520711683322],"translation_xyz_m":[0.06429544616484763,0.052014818558513545,1.0274683645040352],"reprojection_error_px":0.3707704702353167}]}]},{"stamp_ns":1789379977374062006,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0794761094507617}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22227917135835623,0.349260623042639,-0.43153042245243695,0.8014926585254112],"translation_xyz_m":[0.0601120567372152,0.093633600195884,1.0237960677113302],"reprojection_error_px":0.33631643713700143},{"quaternion_xyzw":[0.1229776865854719,-0.3396087790377246,-0.410263962485261,0.8373922897235702],"translation_xyz_m":[0.060172219451535884,0.09362993845141514,1.023760921225607],"reprojection_error_px":0.3881631437601211}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32414144036731535,-0.07288905053453507,0.8042474476108216,-0.4927530374947016],"translation_xyz_m":[0.06429218459110315,0.05199713162350271,1.0276773361566076],"reprojection_error_px":0.16186427038198067},{"quaternion_xyzw":[-0.24812831010612507,0.08250846059579096,0.8181824042818322,-0.5120568806076057],"translation_xyz_m":[0.06434016920713057,0.052007543177766555,1.02755400301038],"reprojection_error_px":0.37334480143500065}]}]},{"stamp_ns":1789379977407212552,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06957316970475587}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2223791337310493,0.349330353027857,-0.4310989091758186,0.8016667361453138],"translation_xyz_m":[0.060116081957699366,0.09363557325076205,1.0238149598529251],"reprojection_error_px":0.34017842595082953},{"quaternion_xyzw":[0.12308541540476249,-0.3396254412823994,-0.409835477581079,0.8375795015783644],"translation_xyz_m":[0.06017632993003822,0.09363208976942412,1.0237812873694356],"reprojection_error_px":0.3899616803820005}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32394319104963937,-0.0734928259325396,0.803224692855729,-0.49445900365535483],"translation_xyz_m":[0.06436645857137423,0.051961088861751006,1.0274190086063493],"reprojection_error_px":0.16412960774440719},{"quaternion_xyzw":[-0.2478511421654919,0.0828861109187023,0.8171472520662659,-0.513780179049936],"translation_xyz_m":[0.06441422963200065,0.05197135656027046,1.027292425705204],"reprojection_error_px":0.3784517542049893}]}]},{"stamp_ns":1789379977440728487,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08771071906198409}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22224748161222452,0.3495083020174732,-0.43089251878182994,0.8017366406718212],"translation_xyz_m":[0.06008623642764525,0.09363094412536052,1.0236331248419797],"reprojection_error_px":0.34746992819910116},{"quaternion_xyzw":[0.12295991954214432,-0.3397977995485703,-0.40959918385158717,0.8376436128785634],"translation_xyz_m":[0.0601466208341935,0.09362764054993536,1.0236015972580166],"reprojection_error_px":0.39418848520855654}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3240509894444485,-0.07406841781708941,0.8024181244223966,-0.49561071348447505],"translation_xyz_m":[0.0643721029512143,0.05192625959077057,1.0268466314976796],"reprojection_error_px":0.16603539792089303},{"quaternion_xyzw":[-0.24792069842198927,0.08332382580464394,0.8163323615030651,-0.5149698466027496],"translation_xyz_m":[0.06441988546570394,0.051936510327663804,1.026719483670369],"reprojection_error_px":0.3803103943262109}]}]},{"stamp_ns":1789379977474418769,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12318671033228495}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22246744780083574,0.3496455923043357,-0.43057042544203655,0.8017888145801215],"translation_xyz_m":[0.060080764929770075,0.09363942800158573,1.0235849337210525],"reprojection_error_px":0.34858254560795926},{"quaternion_xyzw":[0.12318436040554248,-0.3399096529627763,-0.40927398545440774,0.8377242064095245],"translation_xyz_m":[0.06014101892095099,0.09363596190465155,1.0235511448800307],"reprojection_error_px":0.3977812016450221}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.323792637199308,-0.07467189065035934,0.8015326192192049,-0.49711960046835735],"translation_xyz_m":[0.06444335529996202,0.05191450303412989,1.0267502392475285],"reprojection_error_px":0.16399502083068762},{"quaternion_xyzw":[-0.2475842550879808,0.0837428227286907,0.8154246274220078,-0.516499615941426],"translation_xyz_m":[0.06449117096169855,0.0519247877242389,1.0266238822360183],"reprojection_error_px":0.37853360228556676}]}]},{"stamp_ns":1789379977507356121,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08452812510297607}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22248815189658513,0.3497343000225038,-0.4301846377229459,0.8019514443659224],"translation_xyz_m":[0.06010956054512222,0.0936606274478123,1.0236965505764772],"reprojection_error_px":0.3474939892748801},{"quaternion_xyzw":[0.12319511791899264,-0.3399411254625203,-0.40888340396799977,0.8379005645658332],"translation_xyz_m":[0.06016961280877703,0.09365688535668566,1.0236594574593205],"reprojection_error_px":0.4004302134836168}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32284288821059093,-0.07537739185202515,0.8008694512454604,-0.4986971429547449],"translation_xyz_m":[0.06458479908506644,0.051930902030177056,1.0272436974890704],"reprojection_error_px":0.16577568645393495},{"quaternion_xyzw":[-0.24651490060171383,0.0842565449920766,0.8146981993369428,-0.5180715031773785],"translation_xyz_m":[0.06463257405159843,0.05194113969872263,1.0271182029506594],"reprojection_error_px":0.3783798201929313}]}]},{"stamp_ns":1789379977540942998,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03626655277505661}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22246267681248827,0.3499119600983749,-0.4298891396314918,0.8020394661314563],"translation_xyz_m":[0.060122579222808434,0.09364520795432998,1.0234498199081175],"reprojection_error_px":0.3489312776581427},{"quaternion_xyzw":[0.12316188322680952,-0.34006622080176735,-0.40857480274220276,0.8380052186912678],"translation_xyz_m":[0.06018267425876809,0.09364151734208233,1.023413226711344],"reprojection_error_px":0.4011653614913593}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3230813646778877,-0.07596773656585022,0.8001683249580671,-0.49957780828689374],"translation_xyz_m":[0.0646382267074008,0.051896862656729485,1.0267434321879794],"reprojection_error_px":0.15992734377532034},{"quaternion_xyzw":[-0.2466880570722284,0.08470529747478234,0.8140083712509271,-0.5189994090661864],"translation_xyz_m":[0.06468620034754749,0.051907218699042565,1.0266202626212695],"reprojection_error_px":0.3730660026937536}]}]},{"stamp_ns":1789379977573924315,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08301110308110717}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22254033107952878,0.35023031706971586,-0.42932845608529435,0.8021792834794959],"translation_xyz_m":[0.0601432468893066,0.09363123149785084,1.0231956081641957],"reprojection_error_px":0.32523461023563244},{"quaternion_xyzw":[0.12323156676436212,-0.3403018415208505,-0.40799371872856244,0.8381824163559846],"translation_xyz_m":[0.060203383724062795,0.09362761102564665,1.0231594633856913],"reprojection_error_px":0.3791299526042735}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3224947599266086,-0.07648482256653179,0.7991835463854671,-0.5014507562299545],"translation_xyz_m":[0.06477385509055154,0.05188211614715626,1.0267343359840524],"reprojection_error_px":0.1530616145830752},{"quaternion_xyzw":[-0.2459698415083427,0.08497275351804302,0.8130022652219892,-0.5208702189335047],"translation_xyz_m":[0.06482190042093436,0.05189259108190772,1.0266133862698146],"reprojection_error_px":0.3673166151712924}]}]},{"stamp_ns":1789379977607235914,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0658773476470727}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2226620495735204,0.35025509401433597,-0.428793404309639,0.8024208354830265],"translation_xyz_m":[0.060157842505174884,0.09364159564668831,1.0231871454505324],"reprojection_error_px":0.3304106432460321},{"quaternion_xyzw":[0.12334774654493148,-0.3402552550848519,-0.40746596689460385,0.8384409225652545],"translation_xyz_m":[0.060217974739058114,0.09363802585634869,1.0231509258029392],"reprojection_error_px":0.38385593070110924}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32209339272204796,-0.0776301045806874,0.7980024144902242,-0.5034099320587192],"translation_xyz_m":[0.06489395456069572,0.05185780346081274,1.02652343450041],"reprojection_error_px":0.15804646946621578},{"quaternion_xyzw":[-0.24544857215203253,0.08585450060038964,0.8117786467155969,-0.5228761152431619],"translation_xyz_m":[0.06494183466382923,0.05186807298614396,1.0263998328750854],"reprojection_error_px":0.37302947822766336}]}]},{"stamp_ns":1789379977640794980,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0710249645546677}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2228826402519535,0.3502044146253045,-0.42818969065818724,0.8027040460003376],"translation_xyz_m":[0.06016574130365139,0.09367407034270805,1.0234213752798835],"reprojection_error_px":0.3372853073074493},{"quaternion_xyzw":[0.12357127399926775,-0.3401407426214636,-0.40687891352247624,0.8387395097298603],"translation_xyz_m":[0.0602259669079043,0.09367075048764677,1.02338698218239],"reprojection_error_px":0.38793511664563407}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3220002362472331,-0.07833012163633757,0.7967638164438722,-0.5053193650622921],"translation_xyz_m":[0.06497041988172024,0.051818353211291246,1.0259868554647464],"reprojection_error_px":0.1629595047083557},{"quaternion_xyzw":[-0.2452570032886452,0.086307223078083,0.8105329132113681,-0.5248204094578605],"translation_xyz_m":[0.06501820394541015,0.051828569610018145,1.0258615539081668],"reprojection_error_px":0.3777305518635293}]}]},{"stamp_ns":1789379977674100023,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13544177662778759}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2229798235547314,0.3501836291482475,-0.42784954733552244,0.8028674791077616],"translation_xyz_m":[0.060160328556320995,0.09366628446535417,1.023287540179955],"reprojection_error_px":0.3404165061143508},{"quaternion_xyzw":[0.12366909800567208,-0.34007908401433345,-0.4065474519961479,0.8389108058011145],"translation_xyz_m":[0.06022053005618345,0.09366295704157418,1.0232526010812464],"reprojection_error_px":0.3914166601009284}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3213016647493915,-0.07933739633389301,0.7958177346952465,-0.5070946173218115],"translation_xyz_m":[0.06507572581761378,0.05181174556493191,1.026080936016643],"reprojection_error_px":0.16196759262152646},{"quaternion_xyzw":[-0.24445992329634136,0.08708998399490234,0.8095232051633634,-0.5266183256322833],"translation_xyz_m":[0.06512344541187604,0.051821842962251743,1.0259551703969243],"reprojection_error_px":0.37783441141665175}]}]},{"stamp_ns":1789379977707475903,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09818215110641369}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.222912097618361,0.3501685644953239,-0.4274962307014181,0.803081033215669],"translation_xyz_m":[0.06019345559445128,0.09368858924175713,1.023473945623171],"reprojection_error_px":0.33942265255168036},{"quaternion_xyzw":[0.12358783112468469,-0.3400020235447506,-0.4061961517649919,0.8391241614175359],"translation_xyz_m":[0.06025398003457252,0.09368580191762525,1.0234446857674833],"reprojection_error_px":0.3838710690582189}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32071286279603556,-0.07950799557971878,0.7952250175348877,-0.5083688717483634],"translation_xyz_m":[0.06517536957594773,0.0518086103336518,1.0262198095024861],"reprojection_error_px":0.154319277567925},{"quaternion_xyzw":[-0.24377809584700078,0.08709605182372819,0.8089120604766873,-0.5278710033306329],"translation_xyz_m":[0.06522324004267231,0.05181890641669256,1.026097701242792],"reprojection_error_px":0.36995950927016263}]}]},{"stamp_ns":1789379977740784497,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05229863221217289}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22326259768830525,0.34991456643888497,-0.4273018575084165,0.8031978157572479],"translation_xyz_m":[0.06023697330728347,0.0936822701905372,1.0234418292139822],"reprojection_error_px":0.33646284101384977},{"quaternion_xyzw":[0.12391994485397546,-0.33968781269871023,-0.40605625469993145,0.8392701324308032],"translation_xyz_m":[0.06029747335866011,0.09367952615440724,1.0234121222145225],"reprojection_error_px":0.3817055598089563}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.32045360666441036,-0.08009546726003633,0.7942195975237069,-0.5100092479646315],"translation_xyz_m":[0.06531056990024983,0.05178208150875057,1.0260983921464217],"reprojection_error_px":0.1561568973980115},{"quaternion_xyzw":[-0.24339943192654823,0.08743950348961109,0.8079037651860784,-0.5295305052265468],"translation_xyz_m":[0.06535835184019109,0.05179234152410147,1.0259753548517259],"reprojection_error_px":0.37204992180704716}]}]},{"stamp_ns":1789379977774175837,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08767728559256403}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2234310873229694,0.3497542473779578,-0.42681729093331783,0.8034783854087784],"translation_xyz_m":[0.060276739003163445,0.09368802579178398,1.0233528679577966],"reprojection_error_px":0.3773241682874347},{"quaternion_xyzw":[0.12407091833503509,-0.3394345698416808,-0.40560655681227265,0.8395676870215925],"translation_xyz_m":[0.060336741289534694,0.0936845837135373,1.0233147067113808],"reprojection_error_px":0.4286955655413116}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3202839867361647,-0.08107411128241067,0.7926930593777859,-0.5123308207930122],"translation_xyz_m":[0.06545961970954854,0.05173945210528554,1.0255491842003195],"reprojection_error_px":0.15141285754123926},{"quaternion_xyzw":[-0.24307141914270652,0.08808831487223677,0.8063721209747706,-0.5319029389781281],"translation_xyz_m":[0.06550756741580702,0.051749850793202,1.0254286443872773],"reprojection_error_px":0.3672960292891363}]}]},{"stamp_ns":1789379977807598323,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.048802297737122496}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22295510466817706,0.3499702614970647,-0.42582989443261415,0.8040402591771055],"translation_xyz_m":[0.06030300201754646,0.09372026443851367,1.0235048051539664],"reprojection_error_px":0.3554022869761428},{"quaternion_xyzw":[0.12357621914401885,-0.33952037818231656,-0.4045718284396043,0.8401050330131594],"translation_xyz_m":[0.06036344802333848,0.093717518140138,1.0234742905609375],"reprojection_error_px":0.39995767909652}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3196119834513093,-0.08233686540467706,0.7910884595984371,-0.5150222031328591],"translation_xyz_m":[0.0656022904154412,0.051703433428899194,1.025214517464395],"reprojection_error_px":0.15147900834439476},{"quaternion_xyzw":[-0.24224903836031755,0.08899821157896952,0.8047115471409796,-0.5346344991180584],"translation_xyz_m":[0.06565023459269768,0.051713790313533424,1.025094272127559],"reprojection_error_px":0.36738258121126227}]}]},{"stamp_ns":1789379977840655803,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09804617995513189}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22240816304099614,0.3503110052457317,-0.42493465731189795,0.8045168398682991],"translation_xyz_m":[0.060302192082784346,0.0937464022605363,1.0235533008855555],"reprojection_error_px":0.35434975031936083},{"quaternion_xyzw":[0.12302452316240693,-0.3397598249001367,-0.40361000168806066,0.8405517203729076],"translation_xyz_m":[0.06036291466157502,0.09374405910317088,1.023527526030376],"reprojection_error_px":0.39340291842490277}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3182978140409217,-0.08364454168067409,0.7899379917834637,-0.5173857954765567],"translation_xyz_m":[0.06575145153364237,0.05172135370803382,1.025647763484052],"reprojection_error_px":0.15678571712812012},{"quaternion_xyzw":[-0.2408037166468713,0.09002528363143412,0.8034477160527602,-0.5370109737479639],"translation_xyz_m":[0.065799118475712,0.051731395141453515,1.0255245768251344],"reprojection_error_px":0.3733202435536187}]}]},{"stamp_ns":1789379977874260661,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12931744522376945}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22240956525738761,0.3504504318717024,-0.42491235972516195,0.8044675050216459],"translation_xyz_m":[0.060284845258098387,0.09373178236393903,1.0233445428541015],"reprojection_error_px":0.34373625463530105},{"quaternion_xyzw":[0.12302896504778738,-0.33990774191256107,-0.4035711486885548,0.8405099218292277],"translation_xyz_m":[0.06034536923664081,0.09372910060292804,1.0233151781765637],"reprojection_error_px":0.38759956983403365}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31842748956693273,-0.083962939676259,0.7896604884506803,-0.5176779613126823],"translation_xyz_m":[0.06574616425566081,0.051703286423093235,1.025282573584961],"reprojection_error_px":0.15927382607347743},{"quaternion_xyzw":[-0.24092227999836613,0.09030112658991553,0.8031679585685906,-0.5373298724859281],"translation_xyz_m":[0.06579379617532582,0.05171325439356233,1.0251581597319992],"reprojection_error_px":0.37610976483180825}]}]},{"stamp_ns":1789379977907469552,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09475668965562803}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22215134986122195,0.35050589162476053,-0.42399292622118623,0.8049996249723128],"translation_xyz_m":[0.06033165069008992,0.09376488251951112,1.0234685762504185],"reprojection_error_px":0.3446316430157352},{"quaternion_xyzw":[0.12274339554879668,-0.33982380621756647,-0.4026365744363169,0.8410336666882876],"translation_xyz_m":[0.06039232577969275,0.09376248124060675,1.023441909062226],"reprojection_error_px":0.3851382792980162}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31740251775200073,-0.08507932796885272,0.7880314580907789,-0.5205992419667745],"translation_xyz_m":[0.06593947961692745,0.05169418613927607,1.0252589099382703],"reprojection_error_px":0.14764400913933914},{"quaternion_xyzw":[-0.23970652083660507,0.09103985154747753,0.8014745903866315,-0.5402693867536869],"translation_xyz_m":[0.06598738363631898,0.0517043783121802,1.0251400680507816],"reprojection_error_px":0.36436718968894394}]}]},{"stamp_ns":1789379977940930439,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04604049215528345}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2218412842380713,0.3509476369087603,-0.4229224301799128,0.8054556591187854],"translation_xyz_m":[0.06038012886114628,0.09377075682673262,1.0231083326590515],"reprojection_error_px":0.3662956901090587},{"quaternion_xyzw":[0.12240302542571654,-0.3400977735811042,-0.40151465745519205,0.8415087543338139],"translation_xyz_m":[0.0604403461945208,0.09376762669603846,1.0230736324990395],"reprojection_error_px":0.4140687000122981}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31685284779288775,-0.08663544282990601,0.785822096686004,-0.5240059210077839],"translation_xyz_m":[0.0661612060106995,0.051672406643415464,1.0246588844677418],"reprojection_error_px":0.14583976615693722},{"quaternion_xyzw":[-0.23892092518106842,0.09213427153419755,0.7992253613310774,-0.543752599372816],"translation_xyz_m":[0.06620915936660973,0.05168260572082208,1.024540931652719],"reprojection_error_px":0.36302992876899054}]}]},{"stamp_ns":1789379977974024657,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06743656145564067}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22163147265737632,0.3511755853505222,-0.4219882608611954,0.8059039063540916],"translation_xyz_m":[0.06041631351408011,0.09377339781588115,1.0229423724221403],"reprojection_error_px":0.35212815447343493},{"quaternion_xyzw":[0.12217059554995624,-0.34018285260294245,-0.4005537699893651,0.8419659433285889],"translation_xyz_m":[0.0604768951154179,0.09377084417949423,1.022913666081167],"reprojection_error_px":0.3941666482855388}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3159984932776358,-0.08822074661504385,0.7838459049224694,-0.5272074064817596],"translation_xyz_m":[0.06635584319450209,0.051629389198356475,1.0240828575576488],"reprojection_error_px":0.15026211726293898},{"quaternion_xyzw":[-0.2378635581616352,0.09327859825592368,0.7971823463741656,-0.5470103631883733],"translation_xyz_m":[0.06640365511939345,0.051639407154024354,1.0239629947612647],"reprojection_error_px":0.3679707923414849}]}]},{"stamp_ns":1789379978007397337,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04197529815156461}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22155364550764478,0.3513897419387849,-0.42111823546319244,0.8062869608165782],"translation_xyz_m":[0.06040666584225148,0.0937455172765045,1.0225626849819747],"reprojection_error_px":0.3620549038201375},{"quaternion_xyzw":[0.12210064449679618,-0.34028509582120536,-0.39966573588541465,0.8423566855760036],"translation_xyz_m":[0.060467275052716526,0.09374301627260613,1.022534083647177],"reprojection_error_px":0.4032410251464177}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31483757974689447,-0.08940724189785,0.7822878300994013,-0.5300088625237388],"translation_xyz_m":[0.06652941507434378,0.05161461768438937,1.024229034253301],"reprojection_error_px":0.14599849462876124},{"quaternion_xyzw":[-0.23655184235806437,0.09409461188703754,0.7955410026247051,-0.5498217375056321],"translation_xyz_m":[0.06657723953815224,0.05162462323292988,1.0241107379290793],"reprojection_error_px":0.36392754334326005}]}]},{"stamp_ns":1789379978040893496,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12374189570825321}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22091722628708607,0.3518103910123877,-0.41980284737978757,0.8069638140816192],"translation_xyz_m":[0.060412230630287694,0.09375403204670947,1.0223602533936458],"reprojection_error_px":0.34008599426436287},{"quaternion_xyzw":[0.12144962597493311,-0.34054242684271374,-0.3982730577362622,0.8430061775289855],"translation_xyz_m":[0.06047297644845924,0.09375170501491403,1.0223337295728354],"reprojection_error_px":0.3800365759755854}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3140836675955977,-0.09123766515288192,0.7797745026838052,-0.5338341157807103],"translation_xyz_m":[0.06673484354938994,0.05158401107853927,1.0237822258069966],"reprojection_error_px":0.14529894774639518},{"quaternion_xyzw":[-0.23559295917475703,0.09541723895369293,0.7929561638807127,-0.5537255911201137],"translation_xyz_m":[0.06678267071961172,0.05159395931054076,1.0236641696095619],"reprojection_error_px":0.36400845061745124}]}]},{"stamp_ns":1789379978074311731,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.14966303550107396}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22069975905634046,0.3517739440692251,-0.41912363099371597,0.8073921541414051],"translation_xyz_m":[0.06046078650529005,0.09379012212601469,1.0227237003897884],"reprojection_error_px":0.3393363882028648},{"quaternion_xyzw":[0.12122017970843686,-0.3403979119129372,-0.3975925139134139,0.8434187112441709],"translation_xyz_m":[0.060521635684332636,0.09378801703533005,1.0226992918578455],"reprojection_error_px":0.37667994169947516}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3134196345734838,-0.09213186151871133,0.778339267698273,-0.5361602718551952],"translation_xyz_m":[0.06689037804114409,0.05156430856176986,1.023716707528049],"reprojection_error_px":0.14618532222229974},{"quaternion_xyzw":[-0.23479228100783883,0.0959901257429006,0.7914802892264369,-0.5560732256681974],"translation_xyz_m":[0.06693814013456008,0.051574219817621246,1.0235984747152218],"reprojection_error_px":0.3650348176427201}]}]},{"stamp_ns":1789379978107404899,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1462400487862588}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2201838605506824,0.3520210962570673,-0.4180455535767711,0.8079839914736313],"translation_xyz_m":[0.060536620260913686,0.09382888715744765,1.0229251143011393],"reprojection_error_px":0.3506219976819945},{"quaternion_xyzw":[0.12067602126667164,-0.34046478583946166,-0.39647382150747884,0.8439961708170193],"translation_xyz_m":[0.06059759189247126,0.09382699017038436,1.0229030181365226],"reprojection_error_px":0.3843805235561103}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3120257444676372,-0.09396762084594291,0.7760428791245848,-0.5399698795136587],"translation_xyz_m":[0.06714109581064673,0.05152952961775705,1.0233098800845013],"reprojection_error_px":0.13706077347009785},{"quaternion_xyzw":[-0.23315319082232472,0.09729729474614851,0.7890815921898447,-0.5599313055291597],"translation_xyz_m":[0.06718906367664627,0.05153951206362171,1.0231957789271282],"reprojection_error_px":0.3563895095057698}]}]},{"stamp_ns":1789379978140779385,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11227329611930044}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2199527600746697,0.3524044487419615,-0.41704945026080775,0.8083944853099674],"translation_xyz_m":[0.0605659541583969,0.0938318793904341,1.0225513309629295],"reprojection_error_px":0.35285181074992533},{"quaternion_xyzw":[0.1204178532603419,-0.3407043866038553,-0.39543085866469524,0.8444255429466611],"translation_xyz_m":[0.060626771165587436,0.09382972783426832,1.0225262826853774],"reprojection_error_px":0.38973802006476277}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.31107513008292664,-0.09527344774822566,0.7740857712188473,-0.5430897277564536],"translation_xyz_m":[0.06735700677789112,0.051535760496181075,1.0230934363501418],"reprojection_error_px":0.13332548892386845},{"quaternion_xyzw":[-0.23199186223393065,0.0981820319711908,0.7870604575678573,-0.5630949303521708],"translation_xyz_m":[0.06740501915584288,0.051545757317862914,1.0229809392531821],"reprojection_error_px":0.35265537580500783}]}]},{"stamp_ns":1789379978174128468,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05787294817837625}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21935455046448687,0.35240339926172365,-0.41597715038078886,0.8091096561902987],"translation_xyz_m":[0.060610374379716205,0.09388721144280145,1.0227523455237164],"reprojection_error_px":0.3397426785719565},{"quaternion_xyzw":[0.11977477523444706,-0.3405520395465461,-0.3943205918518231,0.845097380436038],"translation_xyz_m":[0.06067136148561106,0.09388534052091725,1.0227303398473166],"reprojection_error_px":0.37352044169548493}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.310431700194298,-0.09657248172369198,0.7720034681782275,-0.5461836324982591],"translation_xyz_m":[0.06753072247506288,0.051519862480610815,1.0224457302163539],"reprojection_error_px":0.13562359333300505},{"quaternion_xyzw":[-0.23115719722948563,0.09906556277756291,0.7849300923546266,-0.5662482799591807],"translation_xyz_m":[0.06757864849166265,0.051529775254631455,1.022332123765208],"reprojection_error_px":0.355680962671847}]}]},{"stamp_ns":1789379978207597603,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07237657664633383}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21913436015017085,0.3522509025438614,-0.4148036890898817,0.8098378438774241],"translation_xyz_m":[0.06065860360357701,0.09398271321578346,1.023210296676485],"reprojection_error_px":0.35403518610706963},{"quaternion_xyzw":[0.1195096954841914,-0.34025087449591607,-0.3931405199802558,0.8458057144756674],"translation_xyz_m":[0.060719710004492965,0.09398114404541803,1.0231907816434291],"reprojection_error_px":0.38399767992770195}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3094843107709311,-0.09853457315422923,0.769608694212411,-0.5497388989989913],"translation_xyz_m":[0.06774335505243895,0.05154329588508848,1.0222144027375868],"reprojection_error_px":0.13163050225433842},{"quaternion_xyzw":[-0.22999563393784433,0.10057283776621928,0.7824347086412141,-0.5698973937356772],"translation_xyz_m":[0.06779138402786464,0.05155315630564393,1.022102586486114],"reprojection_error_px":0.35264465907976933}]}]},{"stamp_ns":1789379978240879179,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1461882160122011}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21877607400171703,0.3527639071769159,-0.4135175068384453,0.8103690065493545],"translation_xyz_m":[0.060654925334802906,0.09397694329957156,1.022674560064796],"reprojection_error_px":0.35727528078073517},{"quaternion_xyzw":[0.1191366685193621,-0.34060703739184445,-0.3917830033534926,0.8463447161627322],"translation_xyz_m":[0.060715792773126734,0.09397496891208082,1.0226505067693101],"reprojection_error_px":0.3920743794461106}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3083045692181333,-0.1006643083118575,0.7671070309465928,-0.5534995869046514],"translation_xyz_m":[0.06794677464401323,0.05153139451354227,1.02148816472188],"reprojection_error_px":0.1450756578556731},{"quaternion_xyzw":[-0.22859443380204963,0.10220165674256378,0.7798160728682357,-0.5737475914460628],"translation_xyz_m":[0.0679943939861889,0.05154077539438752,1.0213701608947483],"reprojection_error_px":0.3670374593190661}]}]},{"stamp_ns":1789379978307759961,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03828632100385487}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21778038796730864,0.35344507736497904,-0.41092683636393174,0.8116572029244877],"translation_xyz_m":[0.06077427782888641,0.09401019348114517,1.0226369586534934],"reprojection_error_px":0.35463831176332294},{"quaternion_xyzw":[0.1181028011673771,-0.3408878511612592,-0.389095421322606,0.8476154519549901],"translation_xyz_m":[0.06083515075165998,0.09400824466561532,1.0226130055576836],"reprojection_error_px":0.3889109591254281}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3061661766414604,-0.10379165275042862,0.7623003271784343,-0.560702930511101],"translation_xyz_m":[0.0684556593577329,0.051509124330279155,1.0208537202048773],"reprojection_error_px":0.14264083058046523},{"quaternion_xyzw":[-0.22599617380011003,0.1042995680911569,0.7748632360227975,-0.5810630731547024],"translation_xyz_m":[0.06850322545566859,0.05151839962901736,1.0207367282468414],"reprojection_error_px":0.36559270347973616}]}]},{"stamp_ns":1789379978340921877,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09462837417225682}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2176474781773256,0.35343729584669725,-0.4101060002409979,0.812111274219214],"translation_xyz_m":[0.060812257766128155,0.09400510291359233,1.022524480839308],"reprojection_error_px":0.36315961697445326},{"quaternion_xyzw":[0.1179573227213426,-0.34074328746159427,-0.3882796625233152,0.8480677954839807],"translation_xyz_m":[0.06087310904834054,0.09400316074714526,1.0225000720374409],"reprojection_error_px":0.3973621935934765}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30495598965764903,-0.10518618441391499,0.7603359789837749,-0.5637613946016082],"translation_xyz_m":[0.06865946801112918,0.05149092187009396,1.020526721628336],"reprojection_error_px":0.1415383309953209},{"quaternion_xyzw":[-0.22459847439738398,0.1052566623846481,0.7728115040462071,-0.5841566053173166],"translation_xyz_m":[0.0687070233583201,0.0515001263036269,1.0204104907518385],"reprojection_error_px":0.364311120201565}]}]},{"stamp_ns":1789379978374269203,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06641002640709129}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21710152814496997,0.3534486258966721,-0.4094206984226278,0.8125981091732867],"translation_xyz_m":[0.0608338571670788,0.0940458340353534,1.022678016733299],"reprojection_error_px":0.36030651614038645},{"quaternion_xyzw":[0.11738377562757048,-0.3406634837935088,-0.3875569924430877,0.8485098806953012],"translation_xyz_m":[0.06089470186149402,0.09404387147857347,1.0226536397898613],"reprojection_error_px":0.3944087157782774}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30447968552732657,-0.10619826078535842,0.7587959314143569,-0.5658999778904078],"translation_xyz_m":[0.06878113811481569,0.05149807057803143,1.0202751178506542],"reprojection_error_px":0.13141696815804838},{"quaternion_xyzw":[-0.22400128683437212,0.10598052595239368,0.7712260361516365,-0.586346273781316],"translation_xyz_m":[0.06882897862143689,0.05150745085556557,1.020163253821158],"reprojection_error_px":0.35456950025006334}]}]},{"stamp_ns":1789379978407858362,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08643632103334653}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21643579891788628,0.35376389219160415,-0.40778572374277033,0.8134601754479492],"translation_xyz_m":[0.06089454453676293,0.09414095229878496,1.023109482657184],"reprojection_error_px":0.35721644609099457},{"quaternion_xyzw":[0.11668064508142852,-0.3407689769181877,-0.385848743481524,0.8493426155487798],"translation_xyz_m":[0.060955407512270215,0.0941390700701396,1.0230858610743883],"reprojection_error_px":0.3902380658775215}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30226945354066803,-0.10803221767977426,0.7559234917199126,-0.5705627853842128],"translation_xyz_m":[0.06910415640155629,0.05155786109455969,1.0206054742734734],"reprojection_error_px":0.1429584494656401},{"quaternion_xyzw":[-0.22150885238678078,0.10721363620518069,0.7681898858695311,-0.5910358396693987],"translation_xyz_m":[0.06915149060631515,0.05156681399559433,1.020488792988139],"reprojection_error_px":0.3663545120330757}]}]},{"stamp_ns":1789379978441065960,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10575721026379835}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21587618861820548,0.35421329999585816,-0.4061910108283493,0.8142108277442428],"translation_xyz_m":[0.0609317683889684,0.09414858348509347,1.0227680313637502],"reprojection_error_px":0.34732836154927565},{"quaternion_xyzw":[0.11609345684978062,-0.34099442554891385,-0.3841841610422526,0.8500868434610049],"translation_xyz_m":[0.06099268863322564,0.09414677195361064,1.02274503415302],"reprojection_error_px":0.3798543960449762}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.3006686896438949,-0.11017289000060988,0.7524426436552143,-0.5755782669506284],"translation_xyz_m":[0.069436922674437,0.05156825164375796,1.0201660248133166],"reprojection_error_px":0.12788910850465143},{"quaternion_xyzw":[-0.21960313126162184,0.1086470120345695,0.7645904646219854,-0.5961306173358214],"translation_xyz_m":[0.06948465290617507,0.05157744277240433,1.020056462657486],"reprojection_error_px":0.3508619468494761}]}]},{"stamp_ns":1789379978474226616,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09379576296885758}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2153743298180997,0.3547389405836171,-0.405099597171924,0.8146585164718786],"translation_xyz_m":[0.0609573450296983,0.09413883919089326,1.02243779996968],"reprojection_error_px":0.34193171486358914},{"quaternion_xyzw":[0.11558188453360782,-0.34136158249180776,-0.3830215183338321,0.8505337232935531],"translation_xyz_m":[0.06101841377674714,0.09413720409802909,1.0224169755070918],"reprojection_error_px":0.37194199881891565}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.30020057534145306,-0.11131809266777636,0.7504211760915602,-0.5782352075780481],"translation_xyz_m":[0.06961685664576574,0.051564817493892585,1.0197193086330887],"reprojection_error_px":0.12679110197456286},{"quaternion_xyzw":[-0.21897170768198293,0.1093985321179954,0.7625354263250426,-0.5988514640581166],"translation_xyz_m":[0.06966462558912849,0.051574025391596307,1.0196104273705586],"reprojection_error_px":0.34972015188038474}]}]},{"stamp_ns":1789379978507812515,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08512162521751965}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21539047622278723,0.35504837574812875,-0.40400224298897713,0.8150642804656649],"translation_xyz_m":[0.0609983133447416,0.09413113162576316,1.0220840259140183],"reprojection_error_px":0.3543706933872358},{"quaternion_xyzw":[0.11558382109168762,-0.34150199224967964,-0.3819068982822593,0.8509781963339417],"translation_xyz_m":[0.06105938287963696,0.09412953552584988,1.0220629108093269],"reprojection_error_px":0.38399095638369457}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.29896458068386517,-0.11313181023510024,0.7478575370968001,-0.581833719559943],"translation_xyz_m":[0.06985093268814394,0.051561993147280556,1.0190830578069392],"reprojection_error_px":0.13926189014263396},{"quaternion_xyzw":[-0.21751239459231908,0.11069333117522943,0.7598679802420265,-0.6025246859963078],"translation_xyz_m":[0.06989831323471486,0.05157079786583784,1.0189689104841435],"reprojection_error_px":0.36261761311154594}]}]},{"stamp_ns":1789379978541007306,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05807981089079148}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21447477163416767,0.3553224370238692,-0.40234033467177716,0.8160078389182867],"translation_xyz_m":[0.061086096579258335,0.0942320722554113,1.0225593260614092],"reprojection_error_px":0.33399387726010743},{"quaternion_xyzw":[0.11461090297058714,-0.34154031852172073,-0.3801626389809825,0.851874943678568],"translation_xyz_m":[0.061147368902906694,0.0942308114708468,1.0225420614879284],"reprojection_error_px":0.359486106153756}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.29786352039552744,-0.11499620666886473,0.7447942251471327,-0.5859477432816318],"translation_xyz_m":[0.07012662279262762,0.051582698391840806,1.0182858325391178],"reprojection_error_px":0.12226040182081407},{"quaternion_xyzw":[-0.21613151111849066,0.11196779332447118,0.7567175047344629,-0.6067363522883185],"translation_xyz_m":[0.07017445469371568,0.051591764115454464,1.0181786258959151],"reprojection_error_px":0.3471212270056344}]}]},{"stamp_ns":1789379978574399402,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06000989551034826}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21404489902772678,0.35546682424672077,-0.4006002944542834,0.8169134116559127],"translation_xyz_m":[0.06112625520855847,0.09428821856050443,1.0224128086952464],"reprojection_error_px":0.3507071989970064},{"quaternion_xyzw":[0.11413205208185338,-0.34145391287202415,-0.37837872563295954,0.852767635443763],"translation_xyz_m":[0.06118758204582806,0.09428709610026262,1.0223963427951823],"reprojection_error_px":0.3742901880564977}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.29602653075370816,-0.11729449437851588,0.7410772226781315,-0.5911132249460158],"translation_xyz_m":[0.0704885563687493,0.05166153953797806,1.0182373655853292],"reprojection_error_px":0.13292400543318475},{"quaternion_xyzw":[-0.21398029073200983,0.11355983014432687,0.7528468627511785,-0.611995262564625],"translation_xyz_m":[0.07053598416494054,0.05167021463556397,1.018125896039219],"reprojection_error_px":0.3577501728911813}]}]},{"stamp_ns":1789379978607691537,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10252201130776907}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21317008418921626,0.35597540642176845,-0.3987982226027191,0.8178019337703545],"translation_xyz_m":[0.06114992388236853,0.09430696677525706,1.0222002865156634],"reprojection_error_px":0.33415012384104636},{"quaternion_xyzw":[0.11323814541825376,-0.3417265575482455,-0.3764791934447504,0.8536178882800255],"translation_xyz_m":[0.06121152269467547,0.09430622308815792,1.0221881299504403],"reprojection_error_px":0.35256197285305574}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.29419886057017697,-0.11921914625320411,0.7380397362731232,-0.5954252037727034],"translation_xyz_m":[0.07077778433671397,0.051707814573522776,1.018144451187239],"reprojection_error_px":0.11666176950488255},{"quaternion_xyzw":[-0.2118990350539301,0.11488704217465356,0.7496603258726352,-0.6163677167860976],"translation_xyz_m":[0.0708256187792272,0.05171668561311367,1.0180403924763008],"reprojection_error_px":0.3413524368891174}]}]},{"stamp_ns":1789379978641130782,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13245245399088643}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21274111274758517,0.35600290682660196,-0.39765509870421323,0.8184580451999182],"translation_xyz_m":[0.061210373559268845,0.09432540850036263,1.022347977525955],"reprojection_error_px":0.3487111195974226},{"quaternion_xyzw":[0.11279905943210876,-0.34156865733692826,-0.37532410047190745,0.8542476480045076],"translation_xyz_m":[0.061271881861680595,0.09432457114625756,1.0223344821740488],"reprojection_error_px":0.3681456935341499}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.29254381388849554,-0.12082003240035169,0.7352679147804659,-0.5993344060044746],"translation_xyz_m":[0.07104951164978916,0.05171708140199904,1.0179551162597253],"reprojection_error_px":0.12177148032289335},{"quaternion_xyzw":[-0.21002575285434238,0.11591294989756347,0.7467678630484488,-0.620315347143745],"translation_xyz_m":[0.07109719042382469,0.051725782341909256,1.0178503328624093],"reprojection_error_px":0.3443869216886413}]}]},{"stamp_ns":1789379978674383059,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.15446201850476712}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2124252866480973,0.356124242436025,-0.3961489045222497,0.8192173502726016],"translation_xyz_m":[0.06127709367231937,0.0943333385055095,1.022182140233984],"reprojection_error_px":0.3632514074708786},{"quaternion_xyzw":[0.11246104711140657,-0.3414519354952118,-0.3738036742330717,0.8550052056911908],"translation_xyz_m":[0.06133830921285458,0.09433210556151583,1.022163636892828],"reprojection_error_px":0.3877740682004408}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2913852767706027,-0.12265524053323393,0.7318622398171161,-0.6036787012815077],"translation_xyz_m":[0.07134989927623754,0.05172301913954915,1.0171908310485624],"reprojection_error_px":0.11221189725396852},{"quaternion_xyzw":[-0.20860057401291177,0.11708599957254641,0.7432778502547238,-0.6247517159210512],"translation_xyz_m":[0.07139783668365511,0.0517318667218488,1.0170902648156719],"reprojection_error_px":0.3352543040053393}]}]},{"stamp_ns":1789379978707835164,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12355439192482646}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21205488342103485,0.35652949976578246,-0.3944052800226937,0.81997799806109],"translation_xyz_m":[0.06133658150924022,0.09438144199988369,1.0220929558429448],"reprojection_error_px":0.3573946596030544},{"quaternion_xyzw":[0.11205338335613715,-0.3416154516033332,-0.37200134679316693,0.8557791306688678],"translation_xyz_m":[0.06139814008469796,0.09438078326865902,1.0220800929830316],"reprojection_error_px":0.3752740279385839}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28955534527481513,-0.12480636260293006,0.7287563782092558,-0.6078611807772699],"translation_xyz_m":[0.07165247649312684,0.05178498342772471,1.01678158198236],"reprojection_error_px":0.11886669837390872},{"quaternion_xyzw":[-0.20649263317615313,0.11863925087281056,0.7400104036283094,-0.6290231499064113],"translation_xyz_m":[0.07170011260910837,0.051793434493282324,1.016677693998404],"reprojection_error_px":0.3435142425195541}]}]},{"stamp_ns":1789379978740942824,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.045419761175992474}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21153182775942797,0.35681575338837224,-0.39291569869730886,0.8207033920338392],"translation_xyz_m":[0.06136529971353078,0.09444541030402541,1.0219371939220314],"reprojection_error_px":0.3622542917843815},{"quaternion_xyzw":[0.11147887906464125,-0.3417163177378044,-0.370440898577975,0.8564904893663148],"translation_xyz_m":[0.06142685071333192,0.09444474687842438,1.021924068656398],"reprojection_error_px":0.3799983466623893}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2886264648348678,-0.12624983338696297,0.7256712728130645,-0.6116836986388945],"translation_xyz_m":[0.07191289120204117,0.051874304919602945,1.016468596504678],"reprojection_error_px":0.116664409034237},{"quaternion_xyzw":[-0.20532021630061886,0.11955489093529212,0.7368472735979491,-0.6329346982295936],"translation_xyz_m":[0.07196053239544635,0.05188276204346031,1.016365542214084],"reprojection_error_px":0.3419353590933767}]}]},{"stamp_ns":1789379978774313264,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.14692714332247042}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21064570382407277,0.35729368408559387,-0.39131979151097895,0.821485502942571],"translation_xyz_m":[0.06139581267952485,0.09450982528896874,1.0218947129582088],"reprojection_error_px":0.33635899515025136},{"quaternion_xyzw":[0.11054716628771376,-0.34199761446361476,-0.3687349512019507,0.8572347936761832],"translation_xyz_m":[0.06145751325684588,0.09450934731930705,1.021883998546925],"reprojection_error_px":0.35135067697967504}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.28621060716893587,-0.1284073474429435,0.7228475916789607,-0.6156999274570225],"translation_xyz_m":[0.07221290060418965,0.051980992001872325,1.0166695179637273],"reprojection_error_px":0.10179421853895015},{"quaternion_xyzw":[-0.2026390204799657,0.12117008800882782,0.7338048231372706,-0.6370131228564542],"translation_xyz_m":[0.07226087520387593,0.0519894520978829,1.0165730044119583],"reprojection_error_px":0.32718970075410525}]}]},{"stamp_ns":1789379978807614701,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1287620812048106}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21016914348959895,0.35732908049602036,-0.3900837629647078,0.8221797353547524],"translation_xyz_m":[0.061451175304210466,0.09456464796902343,1.022175525668884],"reprojection_error_px":0.3369009497609036},{"quaternion_xyzw":[0.11004460431012736,-0.34185549281522715,-0.36747077777141907,0.8578987321230601],"translation_xyz_m":[0.061513029021055765,0.09456445878860821,1.0221676388696084],"reprojection_error_px":0.3480535029270032}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.285230639208073,-0.1297467998422768,0.7201526697837948,-0.6190229257393581],"translation_xyz_m":[0.07243182636916409,0.05201151530612836,1.016187552030481],"reprojection_error_px":0.11871037759312485},{"quaternion_xyzw":[-0.20147178746688657,0.12201634880064727,0.731030880558354,-0.640402202643256],"translation_xyz_m":[0.07247921653475847,0.052019527197287585,1.0160833775859712],"reprojection_error_px":0.3455221377033229}]}]},{"stamp_ns":1789379978840957465,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07778714605947956}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21021438194339162,0.35740596487711424,-0.3888589407965605,0.8227147829333041],"translation_xyz_m":[0.061519107210671525,0.09455530189630997,1.0221048883633788],"reprojection_error_px":0.34853909794658533},{"quaternion_xyzw":[0.11009293415810492,-0.3417285183070688,-0.3662642738781409,0.8584588792159078],"translation_xyz_m":[0.061580516540622826,0.09455452641290572,1.0220895901426112],"reprojection_error_px":0.36837004476422086}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2836224451069221,-0.1314934563728376,0.7167621087988797,-0.6233136120388182],"translation_xyz_m":[0.07278752753421881,0.05205673295041562,1.016207651039539],"reprojection_error_px":0.1131007343044988},{"quaternion_xyzw":[-0.1996088833305876,0.12309028884888058,0.7275234097311232,-0.6447594611789853],"translation_xyz_m":[0.07283496045626289,0.052064732468891935,1.016105676247163],"reprojection_error_px":0.34067754222102525}]}]},{"stamp_ns":1789379978874477529,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1450409934288725}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20929541026604412,0.35792605528999216,-0.38711094235420457,0.8235468951406187],"translation_xyz_m":[0.061579673584187505,0.09457284513016598,1.0220572172143692],"reprojection_error_px":0.3478828099512391},{"quaternion_xyzw":[0.10916327243748561,-0.3419885090478478,-0.36442728173687605,0.8592554893370429],"translation_xyz_m":[0.061641532142110954,0.09457272577576412,1.0220493305457494],"reprojection_error_px":0.35832390007803006}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2818653476641434,-0.13346093324923605,0.7137572830098761,-0.6271288910844198],"translation_xyz_m":[0.0730926480110399,0.052115086674546646,1.0160726939976121],"reprojection_error_px":0.10818869291913302},{"quaternion_xyzw":[-0.19761243926283867,0.12447004027659388,0.7243656840471296,-0.6486531343461373],"translation_xyz_m":[0.07314031942617263,0.05212309112989202,1.0159751192689768],"reprojection_error_px":0.33201154896579227}]}]},{"stamp_ns":1789379978907707451,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09592636383988253}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20862626270967216,0.35818809239712346,-0.38545699890291957,0.8243781140771868],"translation_xyz_m":[0.06162219720486073,0.09462071262258916,1.0218791022692222],"reprojection_error_px":0.347849831103223},{"quaternion_xyzw":[0.10844963486532325,-0.3420273075608573,-0.3627040838032209,0.8600591521358953],"translation_xyz_m":[0.06168382166487968,0.09462023273322227,1.0218671655579072],"reprojection_error_px":0.36283513713365817}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2808743738642613,-0.13520060778352297,0.7097429925896872,-0.631739872282224],"translation_xyz_m":[0.07337268329983461,0.052182997278573416,1.015144956564474],"reprojection_error_px":0.11016366516881007},{"quaternion_xyzw":[-0.19635895298992848,0.12552816497739638,0.7202659063867982,-0.6533780417759009],"translation_xyz_m":[0.07342032204851752,0.052190982024335426,1.0150471990042007],"reprojection_error_px":0.3335340540851627}]}]},{"stamp_ns":1789379978941127206,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07007107506805345}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2081138395530814,0.35829675037866016,-0.3844433171693655,0.8249335757128455],"translation_xyz_m":[0.06164295153342797,0.09468150338063099,1.0219052953528032],"reprojection_error_px":0.35332042689296167},{"quaternion_xyzw":[0.10789644318331874,-0.34201252332601345,-0.36163759977370774,0.8605835449661314],"translation_xyz_m":[0.06170460288610084,0.09468106481084991,1.0218938408331304],"reprojection_error_px":0.36734278300986506}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2791303911083553,-0.1361490559199119,0.7079285030967747,-0.6343397306136305],"translation_xyz_m":[0.07357092302381456,0.052282152243808985,1.015385933531277],"reprojection_error_px":0.11262869888047992},{"quaternion_xyzw":[-0.1944367397815524,0.1261398828474894,0.7183096212825738,-0.655983515152041],"translation_xyz_m":[0.07361829765778585,0.052289871398982314,1.0152864851208492],"reprojection_error_px":0.33752715631687186}]}]},{"stamp_ns":1789379978974333911,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12281005647842905}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20741963732599641,0.3586900516713078,-0.38245023159423663,0.8258634034978796],"translation_xyz_m":[0.06170307837033679,0.09475880434228545,1.021986184403069],"reprojection_error_px":0.3439365145121142},{"quaternion_xyzw":[0.10715799563219461,-0.34214098523536474,-0.3595612117292569,0.8614943094496057],"translation_xyz_m":[0.061765247284910146,0.0947591942900599,1.021983374149085],"reprojection_error_px":0.3469898871495343}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2775883501766041,-0.13743911141120616,0.7043430176210278,-0.6387144213412076],"translation_xyz_m":[0.07389066225367193,0.052396765360556936,1.0151591835276026],"reprojection_error_px":0.12351374244017228},{"quaternion_xyzw":[-0.1926211518377224,0.12679392967448624,0.7146148485947944,-0.6604135139670066],"translation_xyz_m":[0.07393786104368055,0.05240440584735266,1.0150591540009168],"reprojection_error_px":0.34209158515383364}]}]},{"stamp_ns":1789379979008130567,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05366420507263843}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20674755783210433,0.35917409283346613,-0.3805671064428332,0.8266910522446983],"translation_xyz_m":[0.061768196428765576,0.09478348879543282,1.0217873056607083],"reprojection_error_px":0.33785562261334545},{"quaternion_xyzw":[0.10646259452505878,-0.3423523511415791,-0.3576011586284174,0.8623120055887192],"translation_xyz_m":[0.061830153312874295,0.09478355253181928,1.021780792851009],"reprojection_error_px":0.3451664837418516}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2759323021153023,-0.1395245150319815,0.7000942783974569,-0.6436320965501828],"translation_xyz_m":[0.07427521372603284,0.05249171361338644,1.0147899668157503],"reprojection_error_px":0.1156205835393472},{"quaternion_xyzw":[-0.19066308996147813,0.12811257567198012,0.7102260340751682,-0.6654425103665158],"translation_xyz_m":[0.0743223985057934,0.05249926888344597,1.0146908681904998],"reprojection_error_px":0.33879101759869945}]}]},{"stamp_ns":1789379979041266510,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07783731594299945}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.205973083606187,0.3598094715845839,-0.37841174358229784,0.8275969945008177],"translation_xyz_m":[0.06183931851026806,0.09481074784654679,1.0217405880113346],"reprojection_error_px":0.3244412539833237},{"quaternion_xyzw":[0.10568185204611825,-0.342681610713539,-0.35535343042950557,0.8632060005055916],"translation_xyz_m":[0.06190140177424706,0.09481101408956238,1.0217361098656126],"reprojection_error_px":0.328669561749765}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.27339534295453244,-0.14180019812849326,0.696159371145762,-0.6484672853949002],"translation_xyz_m":[0.0746875498750001,0.05260169561066902,1.0148535836962314],"reprojection_error_px":0.10679397180670785},{"quaternion_xyzw":[-0.18781858362385412,0.1296340587494047,0.7060746371258504,-0.6703564702941891],"translation_xyz_m":[0.0747347990838837,0.05260910385972694,1.0147573913210413],"reprojection_error_px":0.3318014917447691}]}]},{"stamp_ns":1789379979074382846,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13896466082592226}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20530364271174054,0.35976407188187537,-0.37660771482268346,0.8286053680783748],"translation_xyz_m":[0.06187725295234304,0.09481219621520294,1.021356813306537],"reprojection_error_px":0.35561441518175085},{"quaternion_xyzw":[0.10498738712149865,-0.3423697346243076,-0.35352431739256573,0.8641650133920039],"translation_xyz_m":[0.0619391298819388,0.09481216520783438,1.0213485920245935],"reprojection_error_px":0.3639852807058334}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2714521114971095,-0.1443058119150522,0.6920632391158463,-0.6530988109587872],"translation_xyz_m":[0.07504372516867656,0.052701484855779565,1.0144514262852837],"reprojection_error_px":0.11562416732371453},{"quaternion_xyzw":[-0.18560117459827385,0.13140573377596027,0.7017860648235821,-0.675115587390762],"translation_xyz_m":[0.07509088013936412,0.05270860024297753,1.0143545774882752],"reprojection_error_px":0.3360062178141467}]}]},{"stamp_ns":1789379979107726492,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09305253277085934}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.1041310155019362,-0.3428000103560937,-0.35111675062263265,0.8650791362312475],"translation_xyz_m":[0.06199322831448579,0.09488658991008957,1.021321911323902],"reprojection_error_px":0.31554146491318436},{"quaternion_xyzw":[-0.20448461472684326,0.36048555298474694,-0.3743250909369952,0.8295281398025662],"translation_xyz_m":[0.0619306230814401,0.09488550517951273,1.0213182050804614],"reprojection_error_px":0.32335055138923563}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2692160323309058,-0.14576161588637343,0.6880812420469232,-0.6578909359562279],"translation_xyz_m":[0.07538733123418138,0.05284867603799144,1.014239842045215],"reprojection_error_px":0.11896098598212834},{"quaternion_xyzw":[-0.18306937430886996,0.13216447854858332,0.6976295139615194,-0.6799494216851382],"translation_xyz_m":[0.075434428497579,0.05285573476446762,1.01414463187755],"reprojection_error_px":0.3347172910743474}]}]},{"stamp_ns":1789379979141085461,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05412622622388749}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10341912777261526,-0.3426294143945333,-0.349640506662637,0.8658297087205459],"translation_xyz_m":[0.062066723931298674,0.09498192084619585,1.021605541235213],"reprojection_error_px":0.3251383928796365},{"quaternion_xyzw":[-0.20383022379146007,0.36051759661499083,-0.37289832124867317,0.830317496148308],"translation_xyz_m":[0.062004298339843326,0.09498107002986872,1.0216045006727397],"reprojection_error_px":0.32948217355628506}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2678040738808828,-0.14726848474424728,0.6849835168012268,-0.6613550885300268],"translation_xyz_m":[0.07567115908366545,0.05298000729207439,1.0140983355499573],"reprojection_error_px":0.11549604628893054},{"quaternion_xyzw":[-0.18141853676003303,0.13314682059641508,0.694399294083938,-0.6834975194253766],"translation_xyz_m":[0.0757182595504031,0.052986976326847866,1.0140041583133734],"reprojection_error_px":0.3322087231977792}]}]},{"stamp_ns":1789379979174521275,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0862160385818185}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10287890790881968,-0.3430650075611264,-0.3479182132951671,0.8664151705458945],"translation_xyz_m":[0.062081247712064,0.09498329147615214,1.0209979923809602],"reprojection_error_px":0.3145899164471041},{"quaternion_xyzw":[-0.20331026235339358,0.3611643932721138,-0.37127138708306906,0.8308927580546085],"translation_xyz_m":[0.062018766654451614,0.09498240640964345,1.0209966504135801],"reprojection_error_px":0.31994331303429807}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.26587924379697175,-0.14874432842182042,0.6817027081228825,-0.6651802539298804],"translation_xyz_m":[0.07598732296818307,0.053120642476184615,1.0142792455265688],"reprojection_error_px":0.10478765631509507},{"quaternion_xyzw":[-0.17925015490064275,0.13404598122423808,0.6909528617228585,-0.6873755885705566],"translation_xyz_m":[0.07603479321418964,0.05312778371418354,1.0141918992531622],"reprojection_error_px":0.3169993261528892}]}]},{"stamp_ns":1789379979207908495,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08026515210914578}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10215518497248476,-0.3430442611768739,-0.34605132095631413,0.8672562691161364],"translation_xyz_m":[0.06214435146726024,0.09504533353547266,1.0211106241522243],"reprojection_error_px":0.32944192704288416},{"quaternion_xyzw":[-0.20261316970088353,0.36139529703068207,-0.36946523611987925,0.8317672643514818],"translation_xyz_m":[0.06208193280660652,0.09504450673578856,1.0211101725940264],"reprojection_error_px":0.3337045879000114}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.2630637774804624,-0.15083675002543181,0.6777678919363226,-0.6698331198736506],"translation_xyz_m":[0.07635220855483882,0.05326699957471608,1.014186910161839],"reprojection_error_px":0.10982096471648686},{"quaternion_xyzw":[0.1761453238528269,-0.1354333138118978,-0.6867635277529108,0.6920885054262117],"translation_xyz_m":[0.07639905098085412,0.05327347964093024,1.014093345815367],"reprojection_error_px":0.3303383734648947}]}]},{"stamp_ns":1789379979241056620,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07152162943309297}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10125265248931692,-0.34299298153473223,-0.343992411410935,0.8682009766600601],"translation_xyz_m":[0.06223502494153074,0.09510666348711774,1.0213550368450361],"reprojection_error_px":0.3320695341651391},{"quaternion_xyzw":[-0.20173460555641798,0.36164033227914255,-0.36747097307527027,0.8327571692499212],"translation_xyz_m":[0.0621725813135779,0.09510576292212473,1.0213539287978892],"reprojection_error_px":0.3375996614785726}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.26098306697085455,0.15267962154699274,-0.6731611853959778,0.6748561257001642],"translation_xyz_m":[0.07677794186933007,0.053421632709517046,1.014391707502841],"reprojection_error_px":0.12036501405459278},{"quaternion_xyzw":[0.1737857864154586,-0.1364664242673077,-0.6819824788949631,0.6971910168439758],"translation_xyz_m":[0.07682455754474495,0.05342790976958706,1.0142972416379612],"reprojection_error_px":0.3354968880967364}]}]},{"stamp_ns":1789379979274455214,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09996184243270557}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.10040479687405114,-0.34327160179307903,-0.3416302823698779,0.8691215302447476],"translation_xyz_m":[0.06229313779805517,0.09512560234887998,1.0210110048226284],"reprojection_error_px":0.307743870659119},{"quaternion_xyzw":[-0.20090723748981987,0.36223770660559484,-0.36520782933175355,0.8336926095582933],"translation_xyz_m":[0.062230332213689565,0.09512416748002521,1.0210043329217255],"reprojection_error_px":0.32190268278093304}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.25962383407961553,0.15465545069948083,-0.6684624556919946,0.6795845066487088],"translation_xyz_m":[0.07711373387215273,0.05354623928334887,1.013428734008517],"reprojection_error_px":0.14572147220255743},{"quaternion_xyzw":[0.17215073913731768,-0.1376539458521132,-0.6771527168808932,0.7020539240162709],"translation_xyz_m":[0.07716102333977053,0.053552918105174714,1.0133433739303712],"reprojection_error_px":0.3297316789117477}]}]},{"stamp_ns":1789379979307922197,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10842941892369329}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0996879523797272,-0.3432316498554769,-0.33955301358834894,0.8700333888138404],"translation_xyz_m":[0.062357167185933725,0.09519786980699434,1.0210695832795134],"reprojection_error_px":0.30982325238537406},{"quaternion_xyzw":[-0.2002272119697364,0.36247031289155407,-0.36319604977554926,0.8346334316851691],"translation_xyz_m":[0.06229446803468996,0.09519654991715544,1.0210645933283682],"reprojection_error_px":0.32204483041375476}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.25672091454854823,0.15684039450456777,-0.6640392754151176,0.6845051522020663],"translation_xyz_m":[0.07752163526589115,0.05373877110170348,1.0135303019628976],"reprojection_error_px":0.103319210983468},{"quaternion_xyzw":[0.16893384196613523,-0.13908512007762477,-0.6724551815768942,0.7070507161314101],"translation_xyz_m":[0.0775686512320377,0.05374503155701583,1.0134436000409022],"reprojection_error_px":0.3173553148936332}]}]},{"stamp_ns":1789379979341453735,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07804354538556062}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09914538578369479,-0.3429449704400231,-0.3377564831663487,0.8709072842770313],"translation_xyz_m":[0.06243343710923978,0.09530035159341091,1.0213001129329329],"reprojection_error_px":0.34521900813441037},{"quaternion_xyzw":[-0.1997384199610137,0.36242973481601914,-0.3614311461212305,0.8355338278766226],"translation_xyz_m":[0.06237084594466223,0.09529910177903747,1.0212966022351186],"reprojection_error_px":0.35476384337698275}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.25342219797557686,0.15880873861786324,-0.659777394264868,0.6893843370202803],"translation_xyz_m":[0.07797274307752881,0.05399131883336913,1.0142731365064155],"reprojection_error_px":0.10652753202601503},{"quaternion_xyzw":[0.16530988557958068,-0.14030756508116918,-0.6678962375047924,0.7119698342188391],"translation_xyz_m":[0.07802014360229288,0.05399763039867258,1.0141947669941342],"reprojection_error_px":0.30298304792557446}]}]},{"stamp_ns":1789379979374655614,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.048281817182334666}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09823260978248684,-0.3432888701893733,-0.3353799107894266,0.8717932217093118],"translation_xyz_m":[0.06248914718765422,0.0953365216921734,1.0209800351976694],"reprojection_error_px":0.3183686432628161},{"quaternion_xyzw":[-0.19885369379534626,0.3630825389121021,-0.3591695822203635,0.836436183823212],"translation_xyz_m":[0.06242632662948483,0.09533495235580151,1.0209731297822442],"reprojection_error_px":0.3336551493336461}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2513654586410939,0.16016403872301355,-0.6555598001165338,0.6938330025108749],"translation_xyz_m":[0.07834747443678262,0.054176010904722065,1.0140582375334908],"reprojection_error_px":0.1300263057953736},{"quaternion_xyzw":[0.16296296980028,-0.1409441421651763,-0.6635080805622395,0.7164738978440236],"translation_xyz_m":[0.07839437080130746,0.05418194842709688,1.0139753871726076],"reprojection_error_px":0.32021180815137834}]}]},{"stamp_ns":1789379979407920567,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0421263635403145}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09732230027201334,-0.3430963161312457,-0.3331148836111203,0.8728389095610266],"translation_xyz_m":[0.06258109763553417,0.09542691104843903,1.0211639742984318],"reprojection_error_px":0.33453222763397783},{"quaternion_xyzw":[-0.19799031707529877,0.36320579642163703,-0.3569676263885902,0.8375294009768509],"translation_xyz_m":[0.06251842391523789,0.09542550980757201,1.0211592614461809],"reprojection_error_px":0.34690713530237727}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.24860513524410394,0.1623974035793493,-0.6508977597633444,0.6986806683857842],"translation_xyz_m":[0.07875259325714395,0.05438382811513708,1.013892082864841],"reprojection_error_px":0.114893275087206},{"quaternion_xyzw":[0.15988844400809588,-0.14239255833284484,-0.6585745883936149,0.7214149681884054],"translation_xyz_m":[0.07879989499626065,0.054389800268012455,1.0138161951017692],"reprojection_error_px":0.30205869006696834}]}]},{"stamp_ns":1789379979441686621,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.045421368095345305}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09636694194154449,-0.3434441593680568,-0.33081854488241663,0.8736810701043511],"translation_xyz_m":[0.06262615190752342,0.09546586939079099,1.0209637796298026],"reprojection_error_px":0.30464065128447854},{"quaternion_xyzw":[-0.19705194829024944,0.3638420546468254,-0.35478877225142114,0.8383999141398589],"translation_xyz_m":[0.06256313703765948,0.0954639809086785,1.0209537435206764],"reprojection_error_px":0.3257559568965828}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.24580557260734984,0.16462705272732395,-0.6462277860204593,0.7034679826122743],"translation_xyz_m":[0.0791479193080282,0.05458799967505462,1.013913157670533],"reprojection_error_px":0.10372089868388276},{"quaternion_xyzw":[0.15680733231051266,-0.1438447715134948,-0.6536251685667231,0.7262880153616839],"translation_xyz_m":[0.07919538652108848,0.05459383392004887,1.0138411690971458],"reprojection_error_px":0.2908792474416623}]}]},{"stamp_ns":1789379979474503806,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06346739664579881}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09574214701509211,-0.34360284556938064,-0.328827217776807,0.8744387838212171],"translation_xyz_m":[0.06268782091593113,0.09553816740177008,1.0211003773516723],"reprojection_error_px":0.30024192938745947},{"quaternion_xyzw":[-0.1964468300477973,0.3642516783481,-0.3528752506469066,0.8391712669447441],"translation_xyz_m":[0.06262503757993129,0.09553658736551318,1.021093982844633],"reprojection_error_px":0.31701825233037684}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2437618071243576,0.16556503493179325,-0.6422638016662952,0.7075772817612642],"translation_xyz_m":[0.07950571871797908,0.054793661595184534,1.0141705698221843],"reprojection_error_px":0.11017798113396123},{"quaternion_xyzw":[0.15450993335974433,-0.14412615050770025,-0.6494967054779318,0.7304165679982292],"translation_xyz_m":[0.0795530702076953,0.05479944727946789,1.0140997338407505],"reprojection_error_px":0.29108768207288244}]}]},{"stamp_ns":1789379979507971587,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07472250136810075}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09496318319957106,-0.34350581015532233,-0.3270940291722609,0.8752115448884323],"translation_xyz_m":[0.06273989436059485,0.0955892470534098,1.021137719484363],"reprojection_error_px":0.3190842636241387},{"quaternion_xyzw":[-0.19568926741514442,0.3643910111061961,-0.35119816192090103,0.8399909241812729],"translation_xyz_m":[0.06267693210875049,0.09558737807085227,1.021128348918943],"reprojection_error_px":0.3393340355014658}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.24159516771015038,0.16675502190943017,-0.6387004491982617,0.7112568268923879],"translation_xyz_m":[0.07984290515430238,0.054986187542442486,1.0145437712749168],"reprojection_error_px":0.10909972929083073},{"quaternion_xyzw":[0.15211458878237805,-0.14472033013444535,-0.6457400329120073,0.7341232783532645],"translation_xyz_m":[0.07989017692291359,0.05499183856539842,1.0144742164282423],"reprojection_error_px":0.2882898054509771}]}]},{"stamp_ns":1789379979541226921,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04772957247891841}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.094096294853407,-0.34377799446792556,-0.32474890627401226,0.8760711875688553],"translation_xyz_m":[0.06278561165131841,0.0956152879838549,1.0205781592301686],"reprojection_error_px":0.30274995670182175},{"quaternion_xyzw":[-0.19485804409774843,0.3649690559473672,-0.3489620113483846,0.8408647010589619],"translation_xyz_m":[0.0627226431455248,0.09561345104124434,1.0205692687542989],"reprojection_error_px":0.3237310572435411}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23871543985887103,0.16932420934413023,-0.6338975571508025,0.7159037211393832],"translation_xyz_m":[0.08023242799012843,0.0552103077398326,1.0142149452374498],"reprojection_error_px":0.11056339087571325},{"quaternion_xyzw":[0.14893913860385058,-0.1465077613663889,-0.6406229584135243,0.7388875651984244],"translation_xyz_m":[0.0802794997437894,0.05521545780868167,1.0141438916837056],"reprojection_error_px":0.2922682113561141}]}]},{"stamp_ns":1789379979574536631,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03626821733000015}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09318443118926258,-0.3436590509720362,-0.3225220199557445,0.8770374365513706],"translation_xyz_m":[0.06287224359695333,0.09571192774332112,1.020765990115793],"reprojection_error_px":0.2952783339480248},{"quaternion_xyzw":[-0.19399620738396955,0.36515350399742025,-0.34680936849899224,0.8418738931457443],"translation_xyz_m":[0.0628089782347768,0.09570959492209934,1.0207521011082816],"reprojection_error_px":0.32396417930594035}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.23586448636647053,0.17131776200748228,-0.6291455207350972,0.7205512349797147],"translation_xyz_m":[0.08063268980766262,0.05545698643982082,1.0142060885725765],"reprojection_error_px":0.10904278020566616},{"quaternion_xyzw":[0.14579311810248602,-0.14772888149453703,-0.6355840331891365,0.743608419157125],"translation_xyz_m":[0.0806800723631332,0.05546211553936754,1.014140901895461],"reprojection_error_px":0.28022001224040427}]}]},{"stamp_ns":1789379979608358509,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06907179247371965}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09211110071477599,-0.3437290360134094,-0.32009579087206597,0.8780117195074284],"translation_xyz_m":[0.0629102871703182,0.09574570848070335,1.0204815766500301],"reprojection_error_px":0.28668013043989216},{"quaternion_xyzw":[-0.19294093316065233,0.3655329535368634,-0.3444833812518252,0.8429060779416789],"translation_xyz_m":[0.06284699622389374,0.09574336403626094,1.0204675735079058],"reprojection_error_px":0.31673547173567546}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2330016863305797,0.17278769666677243,-0.6244623732672495,0.7251905752429026],"translation_xyz_m":[0.08103417261204358,0.05570061914246645,1.0143180054929148],"reprojection_error_px":0.12674959158187668},{"quaternion_xyzw":[0.14264889933334923,-0.14842531904404743,-0.6306396620214428,0.7482745705094734],"translation_xyz_m":[0.08108149952929605,0.05570558511124889,1.0142548299143472],"reprojection_error_px":0.28382523185954334}]}]},{"stamp_ns":1789379979641554634,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08514823069282969}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09156842996182386,-0.3436426241278365,-0.31778308513475456,0.8789419095247982],"translation_xyz_m":[0.06301621501352396,0.09587719377227365,1.021059665814916],"reprojection_error_px":0.277527328283381},{"quaternion_xyzw":[-0.19243436118407442,0.3657507736238706,-0.3422267584604469,0.8438460961709027],"translation_xyz_m":[0.06295295419753585,0.09587475738626589,1.0210455828726774],"reprojection_error_px":0.30877781967341716}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22969939812687015,0.1746468874319106,-0.619466971179261,0.7300666564283647],"translation_xyz_m":[0.08150700952655393,0.05601238950681999,1.015058556341233],"reprojection_error_px":0.11269837432220231},{"quaternion_xyzw":[0.13903748912143052,-0.14947881830166368,-0.6253200949311857,0.7531928294754268],"translation_xyz_m":[0.08155434003757041,0.05601713730098492,1.0149984734770099],"reprojection_error_px":0.2714387236089688}]}]},{"stamp_ns":1789379979674814127,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0583635561716529}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.09061466043806453,-0.34334295202944093,-0.31554304551054213,0.8799643100917987],"translation_xyz_m":[0.0631097262540286,0.0959610124924098,1.0212598376986526],"reprojection_error_px":0.30619963673104406},{"quaternion_xyzw":[-0.19151627283448383,0.3657740039382981,-0.3400394383806335,0.8449284440876142],"translation_xyz_m":[0.063046269920923,0.09595822429082893,1.0212423552479417],"reprojection_error_px":0.3401769282328256}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22640032901184223,0.17714658207965425,-0.6146448509898579,0.7345567960562897],"translation_xyz_m":[0.08189035228130204,0.05625262331416353,1.014744826062753],"reprojection_error_px":0.12113078954171634},{"quaternion_xyzw":[0.13545663654419018,-0.15119585070194172,-0.620137744536289,0.7577733778301371],"translation_xyz_m":[0.08193756284907229,0.05625686649652987,1.0146845609071395],"reprojection_error_px":0.2758924851689408}]}]},{"stamp_ns":1789379979708090702,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04334345065862555}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08995600649766416,-0.34348281306344286,-0.3132994438783105,0.8807785944779525],"translation_xyz_m":[0.06317616006112327,0.09602796800779204,1.0212013456973619],"reprojection_error_px":0.3023180121413161},{"quaternion_xyzw":[-0.19088194044012172,0.36620427213165074,-0.3378784085712719,0.8457521486272952],"translation_xyz_m":[0.06311290181627158,0.09602544232142192,1.0211871346961705],"reprojection_error_px":0.3327450326186202}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.22404301945240862,0.178528424497922,-0.6097537396627996,0.7390079187993646],"translation_xyz_m":[0.0822983869749456,0.056527698195134854,1.0148939188594304],"reprojection_error_px":0.09866787225954057},{"quaternion_xyzw":[0.1328149785377396,-0.15181124950901054,-0.6150156780162781,0.7622789789786351],"translation_xyz_m":[0.0823460057660839,0.05653214464639378,1.0148408461073184],"reprojection_error_px":0.2507943227365461}]}]},{"stamp_ns":1789379979741240066,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04334348979925379}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08889369128118203,-0.34369350572499985,-0.3108870873732474,0.8816586100512172],"translation_xyz_m":[0.06321595618029525,0.09607195072244029,1.0208218932439186],"reprojection_error_px":0.27274077783103523},{"quaternion_xyzw":[-0.18985253875495775,0.36671738067041143,-0.33558586862183226,0.8466737866639431],"translation_xyz_m":[0.06315251735282996,0.09606919858459777,1.0208051862920602],"reprojection_error_px":0.3095833090756595}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2206831612915384,0.180253401725911,-0.6051976646452866,0.7433326578299257],"translation_xyz_m":[0.08268993251039779,0.05681242054715947,1.0151776429445516],"reprojection_error_px":0.12706965106805337},{"quaternion_xyzw":[0.129186628040144,-0.15280776863775108,-0.610121537158374,0.7666239696716193],"translation_xyz_m":[0.08273690993274099,0.0568161470372525,1.0151193364721995],"reprojection_error_px":0.2749136051172958}]}]},{"stamp_ns":1789379979774670064,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04801155611111062}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08826212284965346,-0.34372366580331115,-0.3088009577998894,0.8824430903455912],"translation_xyz_m":[0.06326419935699865,0.09611856692701903,1.020599875877482],"reprojection_error_px":0.287416234256293},{"quaternion_xyzw":[-0.18924101832141302,0.36702099412660966,-0.33356723601970845,0.8474764456367869],"translation_xyz_m":[0.06320074519441245,0.09611576454458759,1.020583121469271],"reprojection_error_px":0.32341517666712527}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.21820078043538596,0.18206351209728402,-0.6006857658890754,0.7472736497685742],"translation_xyz_m":[0.08302231390419709,0.057046927223301416,1.0148718507156211],"reprojection_error_px":0.12083480505197455},{"quaternion_xyzw":[0.12646069493624315,-0.15391904920558996,-0.605335215546198,0.770639926131217],"translation_xyz_m":[0.08306936735503863,0.05705045554888628,1.0148156108624462],"reprojection_error_px":0.26789516188743595}]}]},{"stamp_ns":1789379979808060377,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.046664618480307285}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08743429909694565,-0.3437816626422207,-0.30651143893382354,0.8833007129936514],"translation_xyz_m":[0.06333968239024257,0.09620972528289563,1.0207562446814717],"reprojection_error_px":0.2862696526164475},{"quaternion_xyzw":[-0.1884409920320938,0.3673750149102586,-0.3313649544683597,0.8483648141523961],"translation_xyz_m":[0.06327623294629513,0.09620688795734848,1.0207394167133532],"reprojection_error_px":0.3229108672818772}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2148581525531628,0.1844082993601915,-0.5955395820213826,0.7517726781775995],"translation_xyz_m":[0.0834412941529718,0.05735773874904376,1.0150432393500526],"reprojection_error_px":0.11363404024339802},{"quaternion_xyzw":[0.12283576827110172,-0.15546956779925217,-0.5998119934068818,0.7752200720355636],"translation_xyz_m":[0.08348831303088905,0.05736085021987958,1.0149882529793526],"reprojection_error_px":0.26204100767671495}]}]},{"stamp_ns":1789379979841395428,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06634397823994816}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08633840129670019,-0.3440501937535956,-0.3041364297319119,0.8841245255899018],"translation_xyz_m":[0.06338734346763006,0.0962572583082991,1.020578505605736],"reprojection_error_px":0.2724633004699427},{"quaternion_xyzw":[-0.18735674977301092,0.3679415659204148,-0.32911224603563927,0.8492358811849057],"translation_xyz_m":[0.06332386874483863,0.09625442497932382,1.0205614782262102],"reprojection_error_px":0.3111712828850205}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2116754034837629,0.18671831951835383,-0.5910003852028464,0.7556774030009864],"translation_xyz_m":[0.08382447965138072,0.05764413077977945,1.0154142223141676],"reprojection_error_px":0.10839272297124744},{"quaternion_xyzw":[0.11941306571366696,-0.1570799332467866,-0.5949038166700525,0.7792020682848917],"translation_xyz_m":[0.08387156022713532,0.05764685853316681,1.015361412042863],"reprojection_error_px":0.25494055575679847}]}]},{"stamp_ns":1789379979874631372,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05899844727077748}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08571835753270182,-0.34377332286605905,-0.302091328880957,0.884993273806339],"translation_xyz_m":[0.06344836827949671,0.09629706940958874,1.0204093275484436],"reprojection_error_px":0.28795068415846004},{"quaternion_xyzw":[-0.18675938464755043,0.3679557353848259,-0.3271004359926927,0.850138114553671],"translation_xyz_m":[0.06338471132441852,0.09629390710948071,1.020389507600765],"reprojection_error_px":0.3294499019571431}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20869246687095558,0.1875945479588395,-0.5862763065283971,0.7599577832030241],"translation_xyz_m":[0.08420309548242717,0.057915886010533144,1.015551187813129],"reprojection_error_px":0.10748183110155567},{"quaternion_xyzw":[0.11617738511573919,-0.15720358345210142,-0.5899094313388795,0.7834517926170871],"translation_xyz_m":[0.08425021367244229,0.057918562007251526,1.0155020459002215],"reprojection_error_px":0.24629930419517146}]}]},{"stamp_ns":1789379979908097121,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06128996047464811}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08488268540786977,-0.34398973650684544,-0.29987841549256283,0.8857420204650842],"translation_xyz_m":[0.06349969515063654,0.09634334712094011,1.0200464205523345],"reprojection_error_px":0.28445231844948077},{"quaternion_xyzw":[-0.18595171421581616,0.36845680516154394,-0.32499127778592907,0.850906699981506],"translation_xyz_m":[0.06343641774254859,0.09634078619672024,1.0200330797405066],"reprojection_error_px":0.3178073402108692}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20517766318892608,0.1901902500497022,-0.5811129294147076,0.7642235004116402],"translation_xyz_m":[0.08460848307965799,0.0582387285662181,1.0156421031521534],"reprojection_error_px":0.12445931882294652},{"quaternion_xyzw":[0.11239863903955236,-0.1590146296423984,-0.5843273204043268,0.7878086545163403],"translation_xyz_m":[0.084655345792985,0.058240731623737114,1.0155910977257714],"reprojection_error_px":0.2587900150507359}]}]},{"stamp_ns":1789379979941276458,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04393870903764834}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08424151904206655,-0.34351855246681684,-0.29783167201820127,0.8866761898931483],"translation_xyz_m":[0.06359349573442559,0.09647947635876655,1.0205787344526338],"reprojection_error_px":0.296942634677172},{"quaternion_xyzw":[-0.18536211886123471,0.36826364822408414,-0.3229716540081848,0.8518873640307026],"translation_xyz_m":[0.06353009178847328,0.09647659272681448,1.0205627946353548],"reprojection_error_px":0.333245386792332}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20259813802349455,0.19115185453141798,-0.5762809545519305,0.7683197409928554],"translation_xyz_m":[0.08498949748357362,0.0585614739508061,1.0159041732977108],"reprojection_error_px":0.10371363046435468},{"quaternion_xyzw":[0.10955298520325252,-0.15925328489809176,-0.5792370966426253,0.7919096669161483],"translation_xyz_m":[0.08503675510971856,0.058563678814721416,1.0158605108693486],"reprojection_error_px":0.23161203059614954}]}]},{"stamp_ns":1789379979974658862,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05141807838731639}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08336278733201903,-0.34397022528955673,-0.2950230177766761,0.8875227032501373],"translation_xyz_m":[0.06362191536194915,0.096491697127692,1.0199384684116735],"reprojection_error_px":0.25595941835208147},{"quaternion_xyzw":[-0.1844851712444235,0.36906283137420176,-0.32029574216631196,0.8527417461571261],"translation_xyz_m":[0.06355796507693362,0.09648802368174812,1.0199144543558147],"reprojection_error_px":0.30836121570639874}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1995632852941605,0.19315831706316727,-0.571262539358718,0.7723493191797061],"translation_xyz_m":[0.08535723841261134,0.05886470719865797,1.015778851944168],"reprojection_error_px":0.11193280595111492},{"quaternion_xyzw":[0.10627119711123276,-0.1605056584983971,-0.5738645759800158,0.7960049087096248],"translation_xyz_m":[0.08540428104468775,0.058866411256959565,1.0157338947444587],"reprojection_error_px":0.23883466302456946}]}]},{"stamp_ns":1789379980008044333,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06213190522319025}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08218481519269381,-0.3441474625847566,-0.29251834801611404,0.8883924787064217],"translation_xyz_m":[0.06370635458214782,0.09658285286756281,1.0200793949461513],"reprojection_error_px":0.24454188574670427},{"quaternion_xyzw":[-0.18333043005754637,0.3695652088176023,-0.31791402603449714,0.8536639748150641],"translation_xyz_m":[0.06364251360086047,0.09657935642233023,1.020057032431481],"reprojection_error_px":0.2965257404834653}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19559862941631376,0.19585255533714996,-0.565951102927101,0.7765837378119412],"translation_xyz_m":[0.08574311449734445,0.05919588849516133,1.0155831075459245],"reprojection_error_px":0.10972383714638664},{"quaternion_xyzw":[0.10203679983508718,-0.16240088304974074,-0.5680754505991754,0.800315392261465],"translation_xyz_m":[0.08579030268808768,0.05919710058564869,1.0155410345890565],"reprojection_error_px":0.2308995504186499}]}]},{"stamp_ns":1789379980041559184,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.12458116072507194}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08140946299440918,-0.3437445446039203,-0.29031826097834423,0.8893410452309707],"translation_xyz_m":[0.06380883998436858,0.09670254626577297,1.0207982296604161],"reprojection_error_px":0.2755789168996743},{"quaternion_xyzw":[-0.1825645893924777,0.36946648400582977,-0.31574535874527737,0.8546751174142967],"translation_xyz_m":[0.0637448294955785,0.09669866400788317,1.0207723975594947],"reprojection_error_px":0.32884535429424594}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19217149412731593,0.19726314289946717,-0.5610448939490268,0.7806317930189781],"translation_xyz_m":[0.08614464414942698,0.059519640753449565,1.0161201498446597],"reprojection_error_px":0.11341133879112672},{"quaternion_xyzw":[0.09838227504128114,-0.1630603868176791,-0.5628100841353162,0.8043488344021945],"translation_xyz_m":[0.08619162949601845,0.05952044680278805,1.016078476374633],"reprojection_error_px":0.23174892705291208}]}]},{"stamp_ns":1789379980074684682,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.1357804745772764}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.08008310229187215,-0.3441481087717465,-0.28741830084629344,0.8902468737910676],"translation_xyz_m":[0.0638557786998403,0.09674205981754423,1.0202932777219682],"reprojection_error_px":0.24678752044480698},{"quaternion_xyzw":[-0.18125393096767597,0.3702328562181181,-0.3130057728301805,0.8556296107898094],"translation_xyz_m":[0.06379174322935659,0.0967382252447166,1.0202676109512063],"reprojection_error_px":0.3043652590328468}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18877590686829746,0.19934723707843396,-0.5552320843050171,0.7850743077018588],"translation_xyz_m":[0.08656538677161119,0.059887659517369796,1.0160993547946984],"reprojection_error_px":0.10878596792077386},{"quaternion_xyzw":[0.09471599539743798,-0.16428769443352895,-0.5565946377781876,0.808851557994516],"translation_xyz_m":[0.08661237196937123,0.059888094319635196,1.0160592090256007],"reprojection_error_px":0.2255748332867881}]}]},{"stamp_ns":1789379980108211336,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07670507472933376}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07924354476263352,-0.34423405858008826,-0.2849371721175864,0.8910859562761416],"translation_xyz_m":[0.06392803162014063,0.09683644263534961,1.0201214693458176],"reprojection_error_px":0.24005283160522614},{"quaternion_xyzw":[-0.18046224528992882,0.3706384409020752,-0.3106274104330629,0.8564876741893288],"translation_xyz_m":[0.06386407138438593,0.09683270195585224,1.0200971921304658],"reprojection_error_px":0.29698991969205263}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1852123275681658,0.20079848745215312,-0.5494999438405219,0.7895734119592511],"translation_xyz_m":[0.08701187496514558,0.06030470215594766,1.0164485975033417],"reprojection_error_px":0.10928040429518202},{"quaternion_xyzw":[0.0908578304292998,-0.1648957424684038,-0.550471971934389,0.8133479310111632],"translation_xyz_m":[0.08705871664014572,0.0603047803495058,1.016409566214268],"reprojection_error_px":0.22294684359672157}]}]},{"stamp_ns":1789379980141837712,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0614392909120087}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07817886742318862,-0.34383866776184113,-0.2824651657439297,0.8921190869953652],"translation_xyz_m":[0.06403235968468801,0.09700509762492338,1.0207796138993994],"reprojection_error_px":0.25813179816158366},{"quaternion_xyzw":[-0.1794503980948165,0.37056551369901763,-0.3082250417547492,0.857599019539947],"translation_xyz_m":[0.06396830338870538,0.09700111300643392,1.0207531245343375],"reprojection_error_px":0.3161473081338407}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18140507801498068,0.2028305823199743,-0.5442185606464145,0.7935856039483475],"translation_xyz_m":[0.08738343215881651,0.060680213205769135,1.0164051775759395],"reprojection_error_px":0.11733338478391321},{"quaternion_xyzw":[0.08678634328061159,-0.16616223161198865,-0.5447382398935844,0.8173851560943246],"translation_xyz_m":[0.08743044896535535,0.060679941465314115,1.016369940117119],"reprojection_error_px":0.21771593965144537}]}]},{"stamp_ns":1789379980174853753,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11178655046151977}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07681789200146251,-0.3444118483231917,-0.27960609702509726,0.8929165250511641],"translation_xyz_m":[0.06405160172207085,0.0970551706086069,1.0201582300123],"reprojection_error_px":0.24155920608737214},{"quaternion_xyzw":[-0.17810725279584314,0.3714668975751609,-0.3055530279093038,0.8584448133938831],"translation_xyz_m":[0.0639877801063282,0.09705165608942638,1.0201361459369926],"reprojection_error_px":0.29597172980517494}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17734642019985525,0.2052563008400973,-0.5385687327887265,0.7977228956662089],"translation_xyz_m":[0.08776433622527424,0.061084262259253286,1.016352700523397],"reprojection_error_px":0.10553236072780697},{"quaternion_xyzw":[0.08245848284017102,-0.16779154700353308,-0.5385821804378772,0.8215691269008593],"translation_xyz_m":[0.08781143568942214,0.06108347713896085,1.0163197762693783],"reprojection_error_px":0.2048774486719657}]}]},{"stamp_ns":1789379980208180146,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08591390060077421}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07593938864555773,-0.3446052529714041,-0.2770133499425084,0.8937248082213467],"translation_xyz_m":[0.06411911810059008,0.0971336640349453,1.0202241379769805],"reprojection_error_px":0.21507499288725998},{"quaternion_xyzw":[-0.17723374057032626,0.37198600787739594,-0.30306829923819606,0.8592812212214086],"translation_xyz_m":[0.06405504394982867,0.097129736716279,1.0201980057181788],"reprojection_error_px":0.2803749015501006}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17445960681172262,0.20782451209066835,-0.5329959128597896,0.8014288331724373],"translation_xyz_m":[0.08812363264893348,0.06143823375666454,1.0161987890793005],"reprojection_error_px":0.10803645167896657},{"quaternion_xyzw":[0.07936403305589233,-0.16958477532499283,-0.5326001144803941,0.8253965545666798],"translation_xyz_m":[0.08817038198928852,0.061436723894669754,1.0161616133967155],"reprojection_error_px":0.21735581130091705}]}]},{"stamp_ns":1789379980241647249,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06886687508064968}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07582929876538631,-0.34380714353111963,-0.27525860626319965,0.8945832913618288],"translation_xyz_m":[0.06422395901425802,0.097221170773625,1.0206501420414456],"reprojection_error_px":0.2606873805107134},{"quaternion_xyzw":[-0.17714445907610257,0.3714659759955919,-0.30125981369694166,0.8601600978582773],"translation_xyz_m":[0.06416015814274738,0.09721745508430703,1.020627847280221],"reprojection_error_px":0.31361802525670535}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17074253591086722,0.20935123543308126,-0.5275842885248692,0.8054029209998412],"translation_xyz_m":[0.08850077113080393,0.061781752573891124,1.0162313943932337],"reprojection_error_px":0.1114549838025447},{"quaternion_xyzw":[0.07542891371092254,-0.1703061994417409,-0.526771746037496,0.8293478190630926],"translation_xyz_m":[0.08854757134271173,0.06177991820477638,1.0161970732409569],"reprojection_error_px":0.21173244031079147}]}]},{"stamp_ns":1789379980274779387,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11511465174976175}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07427461322809777,-0.34403496064917855,-0.272290173868895,0.8955340802534439],"translation_xyz_m":[0.06427816860871277,0.09725714573846586,1.0202623087870273],"reprojection_error_px":0.22595186296835842},{"quaternion_xyzw":[-0.17559517399786914,0.37207568854330547,-0.2984473561974754,0.8611940503988752],"translation_xyz_m":[0.06421421647592503,0.09725329800608437,1.0202380577977332],"reprojection_error_px":0.28721668596782685}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16683972319352686,0.21194942351333174,-0.5223006184077867,0.8089771397560732],"translation_xyz_m":[0.08884610681474425,0.06212601024663723,1.0159176041127191],"reprojection_error_px":0.121102385346375},{"quaternion_xyzw":[0.07130948897176961,-0.17214113777242365,-0.5209810003039808,0.8329833028286127],"translation_xyz_m":[0.08889294981104194,0.062123559623467536,1.0158843012089525],"reprojection_error_px":0.2146035287316149}]}]},{"stamp_ns":1789379980308591329,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06415788435482504}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07393422740610597,-0.3439865369643582,-0.2701817818755962,0.896219168031826],"translation_xyz_m":[0.06433780964012883,0.09734345492072215,1.0201194570966703],"reprojection_error_px":0.26521907067782213},{"quaternion_xyzw":[-0.17528251914117088,0.3722986757471691,-0.2963908337282162,0.8618713408637755],"translation_xyz_m":[0.0642741972135338,0.09734003453779665,1.0201006818039389],"reprojection_error_px":0.31350838261832714}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16320405009214606,0.21408283951656498,-0.5168594552125336,0.8126433900644539],"translation_xyz_m":[0.089208238671232,0.0625248375547038,1.0159826764261854],"reprojection_error_px":0.11659505632882498},{"quaternion_xyzw":[0.06744347155143891,-0.1735184218043488,-0.5150709634859604,0.8366867024240692],"translation_xyz_m":[0.08925545147093933,0.06252214350807407,1.0159546888003514],"reprojection_error_px":0.19738504610799776}]}]},{"stamp_ns":1789379980341650296,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03552955426949023}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07260839988062025,-0.3443874325279453,-0.26731676553500466,0.8970323647714665],"translation_xyz_m":[0.06437923527685711,0.09742033879267506,1.0199251878455935],"reprojection_error_px":0.2090006790659558},{"quaternion_xyzw":[-0.1739724448507906,0.3730340684726907,-0.2936947873110116,0.8627412961587558],"translation_xyz_m":[0.0643154655032995,0.09741674220434139,1.0199041897464494],"reprojection_error_px":0.2683348190150568}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15973063777901464,0.21669839920728107,-0.5111413857962187,0.8162489882762687],"translation_xyz_m":[0.0895617127393486,0.0629300304406856,1.015952751747421],"reprojection_error_px":0.10921722517346602},{"quaternion_xyzw":[0.06376164954965678,-0.1753668569713133,-0.5088629341182654,0.8403805279772132],"translation_xyz_m":[0.08960870537735605,0.06292660557475441,1.0159223384875147],"reprojection_error_px":0.19945656978406373}]}]},{"stamp_ns":1789379980374910375,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.039544808915737424}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07179766535359511,-0.3437328114476683,-0.2653830330410032,0.8979224328181209],"translation_xyz_m":[0.06447794137292012,0.09756309393502946,1.0204962620650662],"reprojection_error_px":0.23896064700885503},{"quaternion_xyzw":[-0.17321346312504377,0.372654142285103,-0.2917809385290389,0.8637070512280751],"translation_xyz_m":[0.064413930131414,0.09755902252534764,1.020470809302657],"reprojection_error_px":0.3012546192025751}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15634960619356733,0.2184640184710315,-0.5060484702016402,0.819599425990029],"translation_xyz_m":[0.08984766407452684,0.0632780887341527,1.0154643955653204],"reprojection_error_px":0.12470518069528601},{"quaternion_xyzw":[0.06017211303299173,-0.17642551660970185,-0.5033391487772817,0.8437434771364495],"translation_xyz_m":[0.0898949728154566,0.06327443576841991,1.0154384812050865],"reprojection_error_px":0.1965631892194394}]}]},{"stamp_ns":1789379980408229030,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.055810510722662264}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.07107587443864069,-0.3442439481560919,-0.2625421758491727,0.8986189014988893],"translation_xyz_m":[0.06451077311610676,0.09759307833881191,1.0200351003533117],"reprojection_error_px":0.21112250049120135},{"quaternion_xyzw":[-0.17247003219611165,0.373504036832,-0.2890723614490271,0.8643992088790661],"translation_xyz_m":[0.06444684124482154,0.09758912933646935,1.0200114075592384],"reprojection_error_px":0.2754199068503057}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15296172727248908,0.22055631168090534,-0.5005500240843295,0.8230475665213457],"translation_xyz_m":[0.0902258863028899,0.06367944987192181,1.0157664329543312],"reprojection_error_px":0.10646038656037915},{"quaternion_xyzw":[0.056587159511257985,-0.17775999168829282,-0.49738701702828425,0.8472339901261815],"translation_xyz_m":[0.09027305843531198,0.06367523218527171,1.015739802124233],"reprojection_error_px":0.1867122552304881}]}]},{"stamp_ns":1789379980441818376,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07822280360924813}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0699021633646603,-0.3440671620327036,-0.2600466382798121,0.8995038751929144],"translation_xyz_m":[0.06458854935468089,0.09769662789691864,1.0201891340819838],"reprojection_error_px":0.23419865797046288},{"quaternion_xyzw":[-0.17131799383676982,0.3736566242288954,-0.2866691492195872,0.8653621617816026],"translation_xyz_m":[0.06452434291887846,0.09769224902488759,1.0201609238442455],"reprojection_error_px":0.30237882434890234}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1487099006290089,0.2225038081683789,-0.4942727476096799,0.8270863750394254],"translation_xyz_m":[0.09059801733626174,0.06412218619245673,1.0156081131335704],"reprojection_error_px":0.11376851084565244},{"quaternion_xyzw":[0.05210675154438411,-0.1788416578036002,-0.4905778343324121,0.8512543311721077],"translation_xyz_m":[0.09064534519788008,0.06411756082535175,1.0155848535264418],"reprojection_error_px":0.1811173184223512}]}]},{"stamp_ns":1789379980474967971,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.049524373340812076}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06829107519042829,-0.34412406654255295,-0.2567926038497494,0.9005401237499734],"translation_xyz_m":[0.06465926429415854,0.09778905626401907,1.020077958453063],"reprojection_error_px":0.20581301742051197},{"quaternion_xyzw":[-0.16972593969077224,0.37412495743025354,-0.2835704535712196,0.8664937504017604],"translation_xyz_m":[0.06459473751731556,0.09778424129439274,1.0200448453736568],"reprojection_error_px":0.2876465671369962}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.14434688387723246,0.22574931215288607,-0.48758892730112446,0.830938182508645],"translation_xyz_m":[0.09096178074634001,0.06457179315957323,1.0152266456233594],"reprojection_error_px":0.11901036576131638},{"quaternion_xyzw":[0.04754235603712528,-0.18119646857720162,-0.4832640287224141,0.8551978968055126],"translation_xyz_m":[0.09100939421044509,0.06456658004361583,1.0152061160738826],"reprojection_error_px":0.176106006231043}]}]},{"stamp_ns":1789379980508372066,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04466120528815088}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.067801384495652,-0.3441078819046623,-0.25462023203893513,0.9011999086262706],"translation_xyz_m":[0.06471412498311827,0.09785594596991594,1.0197486273181582],"reprojection_error_px":0.23041769972308804},{"quaternion_xyzw":[-0.1692616556391986,0.37439280709279504,-0.28146390253219894,0.8671554586686284],"translation_xyz_m":[0.0646499633903846,0.09785163854157852,1.0197215735906566],"reprojection_error_px":0.2983931059547015}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.14086274045871597,0.228001170592462,-0.4818394251157352,0.8342684957272839],"translation_xyz_m":[0.09127781293225541,0.06497168273654949,1.0147304624738354],"reprojection_error_px":0.12087125695828564},{"quaternion_xyzw":[0.04385283572584333,-0.18267098081590005,-0.4770218617173334,0.8585792828913871],"translation_xyz_m":[0.09132588358758206,0.0649662476350761,1.0147150393034148],"reprojection_error_px":0.1607490557644414}]}]},{"stamp_ns":1789379980541773738,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03686498564874984}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06656560780035144,-0.34432557524546326,-0.251503363760304,0.9020836857560959],"translation_xyz_m":[0.06479584269249555,0.09799264580872379,1.0199932143901438],"reprojection_error_px":0.20574196446015938},{"quaternion_xyzw":[-0.16804124906138118,0.37498489530737267,-0.2784978789344559,0.8680941183617239],"translation_xyz_m":[0.06473173152867452,0.09798839147968172,1.0199667606224483],"reprojection_error_px":0.27741483379340653}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.13646791793865204,0.2299604451682352,-0.47540458437802907,0.8381438910973878],"translation_xyz_m":[0.09164105831784404,0.06544624048498328,1.0146097892526589],"reprojection_error_px":0.11486282105569028},{"quaternion_xyzw":[0.03923906427478815,-0.18376937103914606,-0.47001945886568525,0.8624214876673263],"translation_xyz_m":[0.09168905993926091,0.06544020301227764,1.0145950512032662],"reprojection_error_px":0.1535045937403568}]}]},{"stamp_ns":1789379980608696482,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0700685601343023}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06445231986586353,-0.34434946419558504,-0.24587616209169036,0.9037777701886579],"translation_xyz_m":[0.06493084347925875,0.09816936305510555,1.0199320911037137],"reprojection_error_px":0.18422735123583434},{"quaternion_xyzw":[-0.1659435163135556,0.3757214917193123,-0.2730903098158625,0.8698952768801447],"translation_xyz_m":[0.0648664718023793,0.09816467577464885,1.019901625399621],"reprojection_error_px":0.26809314730778544}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1284238955830813,0.2357311925558787,-0.4626477032916358,0.8449231980117623],"translation_xyz_m":[0.09233057077950192,0.06637199693388489,1.0141113859296507],"reprojection_error_px":0.11373374176358177},{"quaternion_xyzw":[0.030821636763778283,-0.18788014165306877,-0.45606185360255885,0.8693438127508718],"translation_xyz_m":[0.09237906092271299,0.06636489152430643,1.0141010097866103],"reprojection_error_px":0.13582154967699736}]}]},{"stamp_ns":1789379980641679971,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06363503155434116}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06374786312288276,-0.3439834016661596,-0.24367846823251488,0.9045620119401981],"translation_xyz_m":[0.06498133177356813,0.09823586786203319,1.0196569388438586],"reprojection_error_px":0.2091534257158937},{"quaternion_xyzw":[-0.16526882003244797,0.37564983916593697,-0.2709381290799803,0.870727251020851],"translation_xyz_m":[0.06491722718951175,0.0982315433178259,1.0196308822778708],"reprojection_error_px":0.2807481167120885}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.02674169043608494,-0.18947749050030552,-0.44940298544499696,0.8725939028319251],"translation_xyz_m":[0.09268234977781127,0.06679643327547102,1.0137095400471097],"reprojection_error_px":0.13053285562402822},{"quaternion_xyzw":[-0.1245102344806451,0.23812386116025164,-0.4565516980331084,0.8481478498933975],"translation_xyz_m":[0.09263337028161378,0.066803811828206,1.0137146405759336],"reprojection_error_px":0.13114472313624262}]}]},{"stamp_ns":1789379980674977801,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0457431729886036}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.062292140091225245,-0.3440689172331492,-0.24051244956195836,0.9054777916009703],"translation_xyz_m":[0.06504953178945923,0.09834559980444905,1.019665254879769],"reprojection_error_px":0.19017802376292287},{"quaternion_xyzw":[-0.16382438839529456,0.3761250930483631,-0.2679232260230373,0.8717273823298638],"translation_xyz_m":[0.06498513820422487,0.09834086069501932,1.0196346843633146],"reprojection_error_px":0.27373053595971475}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.12059540916441461,0.24087054804410415,-0.4502885221173792,0.8512804315986282],"translation_xyz_m":[0.092937926946488,0.06726361982556939,1.013218641819224],"reprojection_error_px":0.10699251746296451},{"quaternion_xyzw":[0.02264151764764238,-0.19142988328238006,-0.44253874250268405,0.8757861741596799],"translation_xyz_m":[0.09298679316574761,0.06725546343636848,1.0132115528287335],"reprojection_error_px":0.11552191893018827}]}]},{"stamp_ns":1789379980708584676,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04092693026390222}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06150323311722331,-0.3435790592383488,-0.23823779705546722,0.9063186715627003],"translation_xyz_m":[0.06514340646695325,0.09848836525627438,1.0200154713158462],"reprojection_error_px":0.22178968554179915},{"quaternion_xyzw":[-0.16307890576582568,0.37593936559988694,-0.26569278847150946,0.8726294780952618],"translation_xyz_m":[0.06507966545085439,0.0984845137368394,1.0199947205340458],"reprojection_error_px":0.2832208818945771}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.01906403354224802,-0.19348074014841232,-0.4356017393512123,0.8788929914903868],"translation_xyz_m":[0.09328132194448015,0.0677167694271558,1.0126696347035562],"reprojection_error_px":0.11026500929631057},{"quaternion_xyzw":[-0.11718180274435759,0.2437197374222626,-0.44391667399110823,0.8542874816179141],"translation_xyz_m":[0.09323205372896544,0.06772527896341224,1.0126737956264011],"reprojection_error_px":0.11587310688169408}]}]},{"stamp_ns":1789379980741902570,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04243491650392762}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.06016138637367059,-0.3438972135269072,-0.23545074831229407,0.9070161295353062],"translation_xyz_m":[0.0651820661557184,0.09853374475488498,1.0195790548082109],"reprojection_error_px":0.18390713644042622},{"quaternion_xyzw":[-0.16173684451025733,0.3766010026596788,-0.2630650896024724,0.8733897392092678],"translation_xyz_m":[0.0651179641484416,0.09852943767220249,1.0195531390346422],"reprojection_error_px":0.26122514396571156}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.014915062094785223,-0.19540919983637786,-0.42911964248080203,0.8817307514083673],"translation_xyz_m":[0.09354133524638739,0.06811973300273164,1.0120696778268405],"reprojection_error_px":0.09335200376478708},{"quaternion_xyzw":[-0.1131699329972554,0.24639862395309708,-0.43803973143115504,0.8570889557491327],"translation_xyz_m":[0.09349169145728453,0.06812869468805251,1.0120705345756795],"reprojection_error_px":0.11612757815941083}]}]},{"stamp_ns":1789379980775052047,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04272107960805208}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.059402193477421775,-0.34348199924691825,-0.23301720485417618,0.9078515725851655],"translation_xyz_m":[0.06528378841197861,0.09867675979662621,1.0201943448740343],"reprojection_error_px":0.21451102266434124},{"quaternion_xyzw":[-0.16098575017963787,0.3765088680356231,-0.260674337164093,0.8742845935241532],"translation_xyz_m":[0.06521964097386032,0.09867225354470936,1.0201670524064386],"reprojection_error_px":0.288673485675585}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.01130945807054435,-0.19642799490222873,-0.4228413770782102,0.8845865185539759],"translation_xyz_m":[0.09385849349905782,0.06857603504695879,1.0121417685925083],"reprojection_error_px":0.09871499576798662},{"quaternion_xyzw":[-0.10971329891556326,0.2481622770664194,-0.432259209110468,0.859959564410715],"translation_xyz_m":[0.09380876417705197,0.06858538948450638,1.0121413040184852],"reprojection_error_px":0.12649828661929874}]}]},{"stamp_ns":1789379980808241056,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05364048982426243}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05826720797080233,-0.34392648971233414,-0.2299928741901293,0.9085278091347574],"translation_xyz_m":[0.06530423255130173,0.09872641862314567,1.019669298791912],"reprojection_error_px":0.1803271034494481},{"quaternion_xyzw":[-0.15983921177670124,0.3773042792640551,-0.25781505353003553,0.8749996030861613],"translation_xyz_m":[0.06523977831450173,0.09872150582446991,1.019637784402367],"reprojection_error_px":0.2692623532168327}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.007636224169490385,-0.19873298630653688,-0.4161417140593976,0.8872840368528139],"translation_xyz_m":[0.09408058441424794,0.06900273558320189,1.0112938491049484],"reprojection_error_px":0.09037630652372301},{"quaternion_xyzw":[-0.1061702276792526,0.2512004484661521,-0.42616414373067296,0.8625603399433815],"translation_xyz_m":[0.09403055997025095,0.06901260630214325,1.0112921869131777],"reprojection_error_px":0.1252325883087049}]}]},{"stamp_ns":1789379980841749629,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07484454442445869}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.057769945628953996,-0.3435961527007737,-0.22812789801016384,0.9091545409778903],"translation_xyz_m":[0.06535752809991259,0.09881223103228583,1.019771069148204],"reprojection_error_px":0.18977181743388724},{"quaternion_xyzw":[-0.15936034358213835,0.3772149323400621,-0.255981967291852,0.8756634102978488],"translation_xyz_m":[0.06529332474300677,0.0988076167839494,1.019743326166462],"reprojection_error_px":0.2700795392978607}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[0.004201516110270932,-0.19947685585608743,-0.4104787797163964,0.8897744110968144],"translation_xyz_m":[0.0943355657322595,0.06939887332792502,1.0112802262617369],"reprojection_error_px":0.08746945537201113},{"quaternion_xyzw":[-0.1028504324265379,0.25260417217778447,-0.4209623102904054,0.8651032620809004],"translation_xyz_m":[0.09428558018323804,0.06940918565929144,1.011278342261993],"reprojection_error_px":0.12395632248021683}]}]},{"stamp_ns":1789379981008272638,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06493534872438651}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.053248657052201304,-0.34358217493528237,-0.21625262997136113,0.912332543330198],"translation_xyz_m":[0.06560089721325524,0.09919739187967361,1.019172769995029],"reprojection_error_px":0.19011700485705044},{"quaternion_xyzw":[-0.15486296570757438,0.3786776923981475,-0.24459188764553583,0.8790878657052787],"translation_xyz_m":[0.06553701427350025,0.09919323858054482,1.0191505758395933],"reprojection_error_px":0.26170554821777786}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01222775450720403,-0.20756732962476956,-0.3797824268521977,0.9014053438639074],"translation_xyz_m":[0.09544820227613225,0.07145970323317886,1.0086404981297505],"reprojection_error_px":0.07433277190116475},{"quaternion_xyzw":[-0.0869629327839194,0.2641044908185327,-0.39291025152805725,0.8765316882436491],"translation_xyz_m":[0.09539735575377971,0.07147218119690932,1.0086354927102648],"reprojection_error_px":0.12761344439559805}]}]},{"stamp_ns":1789379981041751706,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.061364842589006666}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.052485515497395094,-0.34368223593200947,-0.21405587734358855,0.9128569837287842],"translation_xyz_m":[0.06565822451165525,0.0992649726692368,1.0190988494791189],"reprojection_error_px":0.17393556996910586},{"quaternion_xyzw":[-0.1540972664462252,0.3790597812690672,-0.24248870441058543,0.879640234943425],"translation_xyz_m":[0.06559399387451657,0.09926028331333372,1.0190713451864182],"reprojection_error_px":0.25899534915618194}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.014945557609860487,-0.20959509928499226,-0.37428301043520507,0.9031936407897283],"translation_xyz_m":[0.09562924865862199,0.07179966068059629,1.0080283843329845],"reprojection_error_px":0.07409403159895812},{"quaternion_xyzw":[-0.08430161781265501,0.26673125053952246,-0.3879112191816768,0.8782212496021584],"translation_xyz_m":[0.09557813698504578,0.07181251771717982,1.00802279038765],"reprojection_error_px":0.12920859668541723}]}]},{"stamp_ns":1789379981075215666,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09347294835699915}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05159903374503101,-0.3433881386792116,-0.21214672439854595,0.9134636792220986],"translation_xyz_m":[0.06572811128338821,0.09935929346323479,1.0192726002960446],"reprojection_error_px":0.19426299481500978},{"quaternion_xyzw":[-0.15322680576372832,0.3790224449165929,-0.2406392703082175,0.880316007937162],"translation_xyz_m":[0.0656641353700517,0.09935496695919405,1.0192488421297574],"reprojection_error_px":0.2684854269829509}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.017736267492000926,-0.21055975275595232,-0.36895728480603557,0.9051080252231567],"translation_xyz_m":[0.09581676529936688,0.07216450740659881,1.0074860806852766],"reprojection_error_px":0.08826137812299693},{"quaternion_xyzw":[-0.08160579171166397,0.2682958460872685,-0.3830223815050259,0.880142993494607],"translation_xyz_m":[0.09576606231529045,0.07217807805303118,1.0074854451474935],"reprojection_error_px":0.1165496611702379}]}]},{"stamp_ns":1789379981108315633,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04917376203670315}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05132323907725838,-0.34364115377664,-0.2115372799672423,0.9135254029011667],"translation_xyz_m":[0.06571832591847956,0.09937539084620158,1.019049993718096],"reprojection_error_px":0.1872687684610114},{"quaternion_xyzw":[-0.15295578697987341,0.37933155652361944,-0.24008819698979622,0.880380460437943],"translation_xyz_m":[0.06565426931944049,0.09937098205193022,1.0190252284306311],"reprojection_error_px":0.2647301976830614}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.018474870780849306,-0.21158079680082495,-0.3674827992473775,0.9054549341806419],"translation_xyz_m":[0.09582686387093158,0.0722555075603064,1.0071332431330349],"reprojection_error_px":0.07773571900761012},{"quaternion_xyzw":[-0.08087683524749767,0.2694434644034477,-0.3817208219469518,0.8804251081737507],"translation_xyz_m":[0.0957756415765545,0.0722689309741162,1.0071284626316386],"reprojection_error_px":0.128094830633592}]}]},{"stamp_ns":1789379981141835673,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04810077403129486}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.051269791235034466,-0.34329326225634027,-0.21155322262180426,0.9136555032365563],"translation_xyz_m":[0.0657384441028291,0.09941898714600132,1.0195256409890616],"reprojection_error_px":0.18927053303686814},{"quaternion_xyzw":[-0.1529110611802436,0.3789852587325245,-0.24007322296969197,0.8805414406177131],"translation_xyz_m":[0.06567429231460668,0.09941437613998327,1.0194988944127523],"reprojection_error_px":0.2699176292293073}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.018773437900278942,-0.21143346987961673,-0.3672405411792708,0.9055814876411873],"translation_xyz_m":[0.0958443382213625,0.07228054675688049,1.0073496655639569],"reprojection_error_px":0.07903969474938333},{"quaternion_xyzw":[-0.08057512575488836,0.2693197505898055,-0.38149999630407705,0.8805863239182278],"translation_xyz_m":[0.09579319762830006,0.07229403083339556,1.0073452433583157],"reprojection_error_px":0.12695473089369025}]}]},{"stamp_ns":1789379981175263063,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05033500565056988}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.051324921530187625,-0.34340774465328233,-0.21142483075029134,0.9136391050542204],"translation_xyz_m":[0.06573318088022079,0.09942074927389641,1.0194677245815744],"reprojection_error_px":0.19439179112260818},{"quaternion_xyzw":[-0.15296049036066167,0.37910884628101005,-0.23995488547196628,0.8805119101967008],"translation_xyz_m":[0.06566879372951254,0.0994157770224077,1.0194373847899572],"reprojection_error_px":0.28042394912442586}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01872208115413252,-0.21111188777737183,-0.3671236379331722,0.9057049679597906],"translation_xyz_m":[0.0958706103939039,0.0723224300184057,1.007578966476947],"reprojection_error_px":0.07480930933974549},{"quaternion_xyzw":[-0.08064977762115386,0.26901573534408724,-0.3813608776987852,0.8807326657208479],"translation_xyz_m":[0.09581933686136423,0.07233573422917956,1.007572668432674],"reprojection_error_px":0.13200207559112864}]}]},{"stamp_ns":1789379981208515006,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05756429916460568}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.050635494802036225,-0.3437409362856598,-0.20981931214689634,0.9139223553660422],"translation_xyz_m":[0.06575493734447165,0.09943147936413753,1.0192691317023714],"reprojection_error_px":0.162306497909585},{"quaternion_xyzw":[-0.1522432834824909,0.3796329820451288,-0.2384473503720406,0.8808198695979481],"translation_xyz_m":[0.0656908169346389,0.09942695929230018,1.0192431928866141],"reprojection_error_px":0.24735516674232827}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.020173212275110354,-0.21179049053672985,-0.3639942532652188,0.906777819101576],"translation_xyz_m":[0.09599714544159522,0.07252569203890016,1.007436251259669],"reprojection_error_px":0.08825383420364168},{"quaternion_xyzw":[-0.07922786015210521,0.27005180276137736,-0.3784758210022671,0.8817885363950408],"translation_xyz_m":[0.09594628405571108,0.07253953090116526,1.0074348902104842],"reprojection_error_px":0.1191170463515528}]}]},{"stamp_ns":1789379981242014157,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07395130840213535}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.05000567546203302,-0.34345854160688954,-0.20756445745047936,0.9145778581521328],"translation_xyz_m":[0.0658180907675353,0.09951655617355352,1.0191646952047873],"reprojection_error_px":0.17079254234879115},{"quaternion_xyzw":[-0.15163129726563546,0.37965045991819923,-0.23624531824107978,0.8815109911865032],"translation_xyz_m":[0.06575407749202128,0.09951213699851826,1.0191404697833655],"reprojection_error_px":0.2508135168825445}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.023730620474482173,-0.21390415428317613,-0.35710176885567,0.9089335493381497],"translation_xyz_m":[0.09619669594245822,0.07296603148347301,1.0065735810022367],"reprojection_error_px":0.08571870281857003},{"quaternion_xyzw":[-0.07574400382670504,0.2729068532765377,-0.3722093142526329,0.8838806037596838],"translation_xyz_m":[0.09614544925142184,0.07298025122429093,1.006570353455514],"reprojection_error_px":0.12543120349735956}]}]},{"stamp_ns":1789379981275183735,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06501592246406426}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.048653019920297226,-0.34327493165964845,-0.20472790809695648,0.9153587758867793],"translation_xyz_m":[0.06591159966089456,0.09968120047832206,1.0196026980156383],"reprojection_error_px":0.15967535593861357},{"quaternion_xyzw":[-0.15030442074712133,0.379820349962714,-0.23353214010211024,0.8823875692672705],"translation_xyz_m":[0.06584765007422339,0.09967685235670895,1.019579027119174],"reprojection_error_px":0.24109921029104842}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.027100818142820563,-0.21558994993897823,-0.34994808772732694,0.9112205304081364],"translation_xyz_m":[0.09641362348008038,0.07346727673911173,1.0058269985461392],"reprojection_error_px":0.09323963508186832},{"quaternion_xyzw":[-0.07247993611574728,0.27536035259162533,-0.36564958634939093,0.8861284980654849],"translation_xyz_m":[0.09636236618738443,0.07348204227750649,1.005825447555025],"reprojection_error_px":0.1227712454883207}]}]},{"stamp_ns":1789379981308734705,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03582389784556916}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04801476058520548,-0.3433678633995247,-0.20201103642292959,0.9159610441027088],"translation_xyz_m":[0.06596700041468147,0.09978306463101638,1.0196553295240574],"reprojection_error_px":0.15828527561580616},{"quaternion_xyzw":[-0.1496495805578325,0.3802376015107996,-0.23091087047464978,0.8830087991253648],"translation_xyz_m":[0.06590305072993877,0.09977863831226554,1.0196315993564642],"reprojection_error_px":0.24034595117839744}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03087826546568299,-0.2170503528813791,-0.3428819042928486,0.9134372867057756],"translation_xyz_m":[0.09662455669216838,0.0739543679721417,1.005319414343373],"reprojection_error_px":0.09026835224931432},{"quaternion_xyzw":[-0.0687757650386968,0.2775742761637863,-0.35919759362276876,0.8883690134685611],"translation_xyz_m":[0.09657299244099628,0.07396946808881737,1.0053157106184714],"reprojection_error_px":0.12981159207650705}]}]},{"stamp_ns":1789379981341942848,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.031498798252449355}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.046904025908527935,-0.34320207348721327,-0.19928535798195826,0.9166775306516748],"translation_xyz_m":[0.06602022633937568,0.0998852151838471,1.0195813466331156],"reprojection_error_px":0.17387914215015518},{"quaternion_xyzw":[-0.1485431389885149,0.38040840937128134,-0.22828829185022803,0.8838036171815294],"translation_xyz_m":[0.06595645784909926,0.09988105451494679,1.019560449116935],"reprojection_error_px":0.24729294693144513}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.033733121334465654,-0.21942727852846672,-0.3354405285248081,0.9155290261841087],"translation_xyz_m":[0.09678323498744608,0.07442451881591172,1.0041559130968618],"reprojection_error_px":0.08365048958320899},{"quaternion_xyzw":[-0.06597453112462354,0.2807213999865698,-0.35236263329309764,0.8903277101670037],"translation_xyz_m":[0.09673093614348344,0.07443965285549084,1.0041480010857244],"reprojection_error_px":0.14272099314072612}]}]},{"stamp_ns":1789379981375174549,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.037940612266288684}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.045972073481641826,-0.34270875499386366,-0.1968803430955783,0.9174286938029146],"translation_xyz_m":[0.0660892273230921,0.10001421292492443,1.019827261077235],"reprojection_error_px":0.18332586376505744},{"quaternion_xyzw":[-0.1476348070695908,0.3802233137177982,-0.2259401892596347,0.8846384721027578],"translation_xyz_m":[0.0660254723199276,0.10001000449797051,1.0198062492901998],"reprojection_error_px":0.2555267191141923}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.037419482057494284,-0.2213142645136261,-0.32844300126854226,0.9174666062604425],"translation_xyz_m":[0.0969585331451931,0.074903557325734,1.0032969531654192],"reprojection_error_px":0.10918545487822819},{"quaternion_xyzw":[-0.06236300495850699,0.2833293662468355,-0.34601472748016293,0.8922494797989744],"translation_xyz_m":[0.09690687483708664,0.07491982230955072,1.003297572286284],"reprojection_error_px":0.12435809668537885}]}]},{"stamp_ns":1789379981442071664,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04142343507761581}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.043817276332951675,-0.34292008700577997,-0.1912403408981418,0.9186473709949169],"translation_xyz_m":[0.06617386351334638,0.10017076132557384,1.0192978262346288],"reprojection_error_px":0.1572775224301517},{"quaternion_xyzw":[-0.14546433037336418,0.38111566855323487,-0.22055967331296628,0.8859708834269482],"translation_xyz_m":[0.06611026549127996,0.1001668268955403,1.0192797653359342],"reprojection_error_px":0.22808594902185098}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.043801318317806974,-0.22409591100252074,-0.3147177058203243,0.9213116914642755],"translation_xyz_m":[0.09733839168177792,0.07585906700554533,1.0020706980402718],"reprojection_error_px":0.10125292277904117},{"quaternion_xyzw":[-0.0561006827958828,0.2875712824262809,-0.33342116570844105,0.8960947478761183],"translation_xyz_m":[0.09728603813003842,0.07587574982047912,1.0020671638562544],"reprojection_error_px":0.1364603907622554}]}]},{"stamp_ns":1789379981475531435,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.030552761538350747}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.042607424096801454,-0.34289122474891814,-0.18851476200601036,0.9192781950573176],"translation_xyz_m":[0.06625738287180503,0.10031634324021575,1.0197229383553374],"reprojection_error_px":0.16930190292760772},{"quaternion_xyzw":[-0.14424537296050158,0.3814204721055585,-0.21795577177012254,0.8866831324607167],"translation_xyz_m":[0.06619371222372875,0.10031227330129992,1.0197033398635018],"reprojection_error_px":0.24162465783490542}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.047593209676070815,-0.22552506484302295,-0.3074254142238334,0.9232350438591379],"translation_xyz_m":[0.09753689726648292,0.07637682664034745,1.0015976516868195],"reprojection_error_px":0.10871744666193618},{"quaternion_xyzw":[-0.05235542153201449,0.28975738505482324,-0.3267704499780484,0.8980649423418886],"translation_xyz_m":[0.09748473450042798,0.0763942067379623,1.001597262274812],"reprojection_error_px":0.1292562056713066}]}]},{"stamp_ns":1789379981508588161,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06500192663882641}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.041899148695893174,-0.34303399300750076,-0.18589131630538774,0.9197915848180716],"translation_xyz_m":[0.06626559773242119,0.10033108392731475,1.0190339108082063],"reprojection_error_px":0.1514411624434898},{"quaternion_xyzw":[-0.14351144697460452,0.38187852538435113,-0.21543530061997848,0.8872208787468363],"translation_xyz_m":[0.06620199032930676,0.10032712325285324,1.0190159498408364],"reprojection_error_px":0.2233714877170845}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.050116805688750383,-0.22745082957492962,-0.300925407258679,0.924769336201969],"translation_xyz_m":[0.09767278892912828,0.07678967464614476,1.000699057967945],"reprojection_error_px":0.11813087061245496},{"quaternion_xyzw":[-0.049851491694182626,0.2923542782934226,-0.32081181836931394,0.8995129692972589],"translation_xyz_m":[0.09762022049050754,0.07680721926092726,1.0006972684318334],"reprojection_error_px":0.14104733072808892}]}]},{"stamp_ns":1789379981541943762,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08134679780399891}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.04104102326237865,-0.34243301950339783,-0.1836037043984244,0.9205134117950365],"translation_xyz_m":[0.06632928979984211,0.10043355092628978,1.019049109527734],"reprojection_error_px":0.17089148772671306},{"quaternion_xyzw":[-0.1426756592852181,0.3815845465817781,-0.21318638003692952,0.8880250319806627],"translation_xyz_m":[0.06626586684224514,0.10042981347329456,1.0190338554994132],"reprojection_error_px":0.23394396494041922}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0528416229631921,-0.2292361678078871,-0.29382208349644295,0.9264594570196687],"translation_xyz_m":[0.09784497524682102,0.07727585304626912,1.0000149486612362],"reprojection_error_px":0.11528513814511983},{"quaternion_xyzw":[-0.04715191137256417,0.2948764495699999,-0.3142777872820148,0.9011404158975591],"translation_xyz_m":[0.09779182107685433,0.07729338934096408,1.000009665731356],"reprojection_error_px":0.15236858949299945}]}]},{"stamp_ns":1789379981575355811,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.057227059093919376}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03986436018812276,-0.3425892023979336,-0.1807116290724208,0.9210791379165697],"translation_xyz_m":[0.0663853553527343,0.10055403914603694,1.0191329292062192],"reprojection_error_px":0.1514227113307078},{"quaternion_xyzw":[-0.14148879674278939,0.3820808492430854,-0.21043976419761445,0.8886564300574281],"translation_xyz_m":[0.06632159407590385,0.10054979151922955,1.019112418517086],"reprojection_error_px":0.22958639501269962}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.056167653497285745,-0.23090225618435029,-0.286695130339892,0.9280814862009574],"translation_xyz_m":[0.09797454276395288,0.0777575051092352,0.9990143310072288],"reprojection_error_px":0.13690609945186788},{"quaternion_xyzw":[-0.04386548115208873,0.2972621476597235,-0.3077782704904917,0.9027644052275801],"translation_xyz_m":[0.09792175615713229,0.07777587031948181,0.9990149026832119],"reprojection_error_px":0.14695794122737554}]}]},{"stamp_ns":1789379981608722663,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0502764442741488}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.039160579377699366,-0.3424438033783273,-0.1788125337724468,0.9215339214138925],"translation_xyz_m":[0.06641079101550867,0.10063224693011712,1.0190534266866536],"reprojection_error_px":0.15542057210247753},{"quaternion_xyzw":[-0.1407913413978356,0.3821606249554345,-0.20860999785720766,0.8891641714076686],"translation_xyz_m":[0.06634702676760378,0.10062797890084398,1.0190329063268504],"reprojection_error_px":0.2329814388766922}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.058086485838390364,-0.2319202742535082,-0.2816867229002112,0.929242453127742],"translation_xyz_m":[0.09807080825567692,0.07811426254875896,0.9984713125743322],"reprojection_error_px":0.13389973268117203},{"quaternion_xyzw":[-0.041968706949189034,0.2987836648307525,-0.3031601823935137,0.9039141845764171],"translation_xyz_m":[0.09801804965509742,0.07813293432648706,0.9984736744718029],"reprojection_error_px":0.1381044039136734}]}]},{"stamp_ns":1789379981641949150,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06856086221964307}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03871444257679836,-0.343065780133206,-0.1765268063976378,0.9217620891860401],"translation_xyz_m":[0.06641507371214007,0.10066526133305383,1.0185543473226826],"reprojection_error_px":0.13470142035506505},{"quaternion_xyzw":[-0.14030981609669266,0.38303095950315,-0.20645676513242062,0.8893683397221114],"translation_xyz_m":[0.06635133799568176,0.10066104353176808,1.018534822070518],"reprojection_error_px":0.21402285568182894}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.06001513805573212,-0.23252949542189078,-0.2770069679031218,0.9303737725753619],"translation_xyz_m":[0.09818658096457096,0.07846947137593963,0.9981210573517215],"reprojection_error_px":0.13038441152715732},{"quaternion_xyzw":[-0.040082096841145116,0.2998839993709972,-0.2988366334897015,0.9050744051836924],"translation_xyz_m":[0.0981333959369991,0.07848803118356053,0.9981199240868038],"reprojection_error_px":0.14889084512624726}]}]},{"stamp_ns":1789379981675254888,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05643780327800019}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03745911308731425,-0.34243894550761755,-0.1743984964657833,0.9224519217156072],"translation_xyz_m":[0.06650003850192367,0.10080190555913333,1.018943331104098],"reprojection_error_px":0.1715980826349763},{"quaternion_xyzw":[-0.1390761660013784,0.3826947300314532,-0.204386330019478,0.8901846953063611],"translation_xyz_m":[0.06643638581921929,0.10079780029211959,1.0189246033062203],"reprojection_error_px":0.24270491940307595}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03763157151358281,0.3029582501633643,-0.29251955633439,0.9062187774733551],"translation_xyz_m":[0.09822390373125843,0.0789191253046075,0.9968981692684465],"reprojection_error_px":0.1411303774813214},{"quaternion_xyzw":[-0.062454161083360375,-0.23492679705450384,-0.27009176499201915,0.9316433417744054],"translation_xyz_m":[0.0982768644050005,0.07889986607916705,0.9968933746646776],"reprojection_error_px":0.14760919458551128}]}]},{"stamp_ns":1789379981708713798,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.037399234313196354}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03694837729264601,-0.3424532916820014,-0.17306292329255724,0.9227186922421354],"translation_xyz_m":[0.06653926364963683,0.10085906630567963,1.0189875580271595],"reprojection_error_px":0.15306261577139363},{"quaternion_xyzw":[-0.13856754073100963,0.3828782116883582,-0.2031114292287406,0.8904768716742413],"translation_xyz_m":[0.06647553917670095,0.10085484436366197,1.018967698605249],"reprojection_error_px":0.22990881115448566}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03558845148925527,0.3036074834188899,-0.2893894063110332,0.907088600770395],"translation_xyz_m":[0.09835347957088658,0.07920931521710499,0.9970866162574662],"reprojection_error_px":0.13618739116366266},{"quaternion_xyzw":[-0.06452142152288078,-0.23521269941837958,-0.2666476467359647,0.9324221172232874],"translation_xyz_m":[0.09840617282657013,0.0791896259594456,0.9970793577069943],"reprojection_error_px":0.15251193153836295}]}]},{"stamp_ns":1789379981741849633,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05274483226180028}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03628829982509455,-0.342232430922006,-0.17148349712507627,0.9231216240207856],"translation_xyz_m":[0.0665880032564127,0.10093031571989781,1.0192552532701267],"reprojection_error_px":0.1566999999834972},{"quaternion_xyzw":[-0.13789179431689771,0.3828596298151778,-0.20157580264127653,0.8909385796492238],"translation_xyz_m":[0.06652411554120878,0.10092581208573262,1.0192325987170636],"reprojection_error_px":0.23917107748885194}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03395813979868608,0.30478969196547223,-0.2852965060960057,0.9080506549870537],"translation_xyz_m":[0.09844264442754944,0.07950152112100349,0.9966333171986644],"reprojection_error_px":0.13819469792986583},{"quaternion_xyzw":[-0.06614583597260185,-0.23593128717919762,-0.26222931408530487,0.9333793135418221],"translation_xyz_m":[0.09849553291346531,0.07948177523642079,0.9966268659127949],"reprojection_error_px":0.15088658620855788}]}]},{"stamp_ns":1789379981775178069,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08022430884608661}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.035596937966474014,-0.34197544586408957,-0.17027128560894883,0.9234681054213751],"translation_xyz_m":[0.06661352543184362,0.10098437671640684,1.0192692289992218],"reprojection_error_px":0.15695544767934266},{"quaternion_xyzw":[-0.13721076249559652,0.38275756323443355,-0.2004067396722543,0.8913512176107423],"translation_xyz_m":[0.06654953268458198,0.10097972815809866,1.019244910134451],"reprojection_error_px":0.2430736303719785}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.03269425172511955,0.30634744468251296,-0.28189478063994006,0.9086350541821449],"translation_xyz_m":[0.09847252688422733,0.07972821762393659,0.9959247185280011],"reprojection_error_px":0.14577385819002137},{"quaternion_xyzw":[-0.06739852825952501,-0.23713104056907014,-0.25851482481800775,0.9340216235914428],"translation_xyz_m":[0.0985255824043404,0.07970834941921846,0.9959180459859455],"reprojection_error_px":0.15948432114677896}]}]},{"stamp_ns":1789379981808524490,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05912119376713535}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03539455341404219,-0.3421635451400886,-0.16945727887206405,0.9235559347451905],"translation_xyz_m":[0.06661414107760545,0.1010046335067123,1.0191228938194774],"reprojection_error_px":0.15554011198332193},{"quaternion_xyzw":[-0.13699928867834085,0.3830307462979279,-0.19964102707154632,0.8914382214163241],"translation_xyz_m":[0.06655034278926227,0.10100028415745918,1.019101716900751],"reprojection_error_px":0.23510206243462495}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.031722518015477146,0.30687177759455586,-0.2800432632317493,0.9090649947529761],"translation_xyz_m":[0.09853816720540896,0.07989752403356586,0.996034419352764],"reprojection_error_px":0.1496883476256415},{"quaternion_xyzw":[-0.06837502913988087,-0.2374520672457272,-0.25648901256409384,0.9344275025836661],"translation_xyz_m":[0.09859135574072549,0.07987765430230706,0.9960288937185362],"reprojection_error_px":0.1588965033806088}]}]},{"stamp_ns":1789379981942189329,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04067747725666879}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.033161759247997635,-0.341701108484301,-0.1626888607490586,0.9250259373511379],"translation_xyz_m":[0.066776196046113,0.10131369869549504,1.0193245476282364],"reprojection_error_px":0.1615430024126089},{"quaternion_xyzw":[-0.13476845528075831,0.3834023778501677,-0.1931044805768475,0.8930569633014579],"translation_xyz_m":[0.06671323081355451,0.1013104794040444,1.019315793060926],"reprojection_error_px":0.21074274270149643}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.024280310982886237,0.31234997754051125,-0.2633126058576176,0.9124222868965266],"translation_xyz_m":[0.09884907926296148,0.08117405854829263,0.9939285234905234],"reprojection_error_px":0.14776381424390692},{"quaternion_xyzw":[-0.07582440609494162,-0.24109375433259458,-0.23824722772110066,0.937743418822747],"translation_xyz_m":[0.09890228532609992,0.08115313724331746,0.9939179135907652],"reprojection_error_px":0.1746643585134947}]}]},{"stamp_ns":1789379981975685181,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06939733589189515}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.03248376014324847,-0.34163924133307555,-0.16151216645886662,0.9252790142406127],"translation_xyz_m":[0.06680127950043845,0.10134377433715232,1.0192681553991978],"reprojection_error_px":0.15813204636816236},{"quaternion_xyzw":[-0.13408014462190124,0.3834925671789472,-0.19198404713260647,0.8933633591004592],"translation_xyz_m":[0.06673815990205481,0.10134035738095773,1.0192570887215144],"reprojection_error_px":0.2135123273747254}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.02314013843545736,0.31332416733800217,-0.2604995221600636,0.9129252428920529],"translation_xyz_m":[0.09891478224514028,0.08138779230606022,0.9937638952736665],"reprojection_error_px":0.15465009772571683},{"quaternion_xyzw":[-0.07694750660880943,-0.24175972529724138,-0.23518945383255654,0.9382522247545345],"translation_xyz_m":[0.09896809011792077,0.08136680144552887,0.9937533880070325],"reprojection_error_px":0.18051187975821512}]}]},{"stamp_ns":1789379982008872617,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08935780357154897}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0322651505366594,-0.34182257747242606,-0.16029815736629788,0.9254300548047506],"translation_xyz_m":[0.06680015301767293,0.10135099482776924,1.0191200959211433],"reprojection_error_px":0.15040782385627627},{"quaternion_xyzw":[-0.1338223613745275,0.3838079315647874,-0.19081821090764814,0.8935163443665197],"translation_xyz_m":[0.0667369145656799,0.1013473800004782,1.019107373955827],"reprojection_error_px":0.2107647639152618}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.02174767277276616,0.3137459206746101,-0.2581377077814514,0.9134853363959095],"translation_xyz_m":[0.09895472638171543,0.08155923592206747,0.993515475002871],"reprojection_error_px":0.13599506749056503},{"quaternion_xyzw":[-0.07832541693963338,-0.2419188669302657,-0.23261300944870367,0.9387393561152245],"translation_xyz_m":[0.09900762748104294,0.08153775684584501,0.9935007660408315],"reprojection_error_px":0.1782856503303353}]}]},{"stamp_ns":1789379982075663827,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05515268247831052}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.031032416117003758,-0.34155222074039004,-0.15757816000711666,0.9260389803599093],"translation_xyz_m":[0.06684886495799428,0.10147732261267232,1.0191233869100793],"reprojection_error_px":0.14858735517137006},{"quaternion_xyzw":[-0.13260192730333709,0.38386568724422454,-0.18820891331629722,0.8942266312184278],"translation_xyz_m":[0.06678570423491213,0.10147382962564333,1.0191117863965347],"reprojection_error_px":0.20622148208353464}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.01837086021382422,0.31590380905901483,-0.2513580462641214,0.9147056507397849],"translation_xyz_m":[0.09904826705812332,0.08208294512027885,0.9926186144469652],"reprojection_error_px":0.14589388194662645},{"quaternion_xyzw":[-0.08169499907210573,-0.24335248098340737,-0.22517792389336955,0.9398938236401497],"translation_xyz_m":[0.0991012354587341,0.08206104909099421,0.9926026847326208],"reprojection_error_px":0.19021675078708467}]}]},{"stamp_ns":1789379982108835542,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.039335189448771804}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.030331699535417707,-0.3413722767376921,-0.15612694283769152,0.9263742949792886],"translation_xyz_m":[0.06689399494708262,0.10155597560741127,1.0192665326451031],"reprojection_error_px":0.14418302516115333},{"quaternion_xyzw":[-0.13191001541657119,0.3838708562908831,-0.1868154951939092,0.8946188486046774],"translation_xyz_m":[0.0668309266225498,0.10155261771494337,1.0192561725818727],"reprojection_error_px":0.19870163322848144}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.017401519199465008,0.317071929242498,-0.24788010563422827,0.9152693767666189],"translation_xyz_m":[0.09914947070197963,0.08238755514885628,0.9926090446115976],"reprojection_error_px":0.15427279269526847},{"quaternion_xyzw":[-0.08266105252011172,-0.2441417472936169,-0.2214340546350639,0.9404939750325689],"translation_xyz_m":[0.0992028652113873,0.08236590588064492,0.9925961203690066],"reprojection_error_px":0.1871536317811205}]}]},{"stamp_ns":1789379982142006299,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.046828891227954875}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.030342476686863285,-0.34147341571649015,-0.1557144123543801,0.9264061000730459],"translation_xyz_m":[0.06689470146185907,0.10156430576738039,1.019083384289278],"reprojection_error_px":0.15967355484068813},{"quaternion_xyzw":[-0.13191665093614055,0.3840198004007758,-0.18642112749913753,0.8946362128417846],"translation_xyz_m":[0.06683179977587878,0.1015611940825986,1.0190757412496436],"reprojection_error_px":0.20618010022180994}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.016833422158388896,0.3173326283871219,-0.2467846207581451,0.9154856578978269],"translation_xyz_m":[0.09916050956644305,0.08247290418890689,0.99236690665998],"reprojection_error_px":0.15198417735583522},{"quaternion_xyzw":[-0.08323737371734,-0.24427992184226868,-0.22023452879095648,0.940688902735437],"translation_xyz_m":[0.09921384208821912,0.08245112763385284,0.9923531168263522],"reprojection_error_px":0.18791146894186503}]}]},{"stamp_ns":1789379982175326237,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04263933965308824}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02976340316043789,-0.3413759261343861,-0.15396330323285057,0.9267534289905965],"translation_xyz_m":[0.06692355202303657,0.10162657215073279,1.0191565836322587],"reprojection_error_px":0.16149782664359266},{"quaternion_xyzw":[-0.13131255446942341,0.384130562255896,-0.18472609356167663,0.8950290467560965],"translation_xyz_m":[0.06686052451814246,0.1016232371190309,1.0191469187961681],"reprojection_error_px":0.21326627577338145}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.015052491889735167,0.3186750636708671,-0.24244492427170783,0.9162096293846534],"translation_xyz_m":[0.09922342397505639,0.08279112238710938,0.9918744018857742],"reprojection_error_px":0.1341415811194815},{"quaternion_xyzw":[-0.08498231973503156,-0.24515952035854519,-0.21552320299484254,0.9413950094837261],"translation_xyz_m":[0.09927626981189772,0.08276870854032882,0.9918543655774872],"reprojection_error_px":0.19347296625801796}]}]},{"stamp_ns":1789379982208988589,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06618547083073911}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.028844905731050453,-0.3415338522411683,-0.152042557329955,0.927041347485236],"translation_xyz_m":[0.06694494012497727,0.10168846883641368,1.019083152300772],"reprojection_error_px":0.15094391309433133},{"quaternion_xyzw":[-0.1303733784296867,0.38450653074383007,-0.18291654242662325,0.8953764842337905],"translation_xyz_m":[0.06688183178365016,0.10168505413963097,1.019072354346725],"reprojection_error_px":0.20637528095345528}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.013605822416711544,0.3201723703038146,-0.23825062514154582,0.9168103263544017],"translation_xyz_m":[0.09926736160807753,0.08310080052135033,0.9913005952691625],"reprojection_error_px":0.14953771447876535},{"quaternion_xyzw":[-0.0864035257609328,-0.24621787583871269,-0.21097361647331106,0.9420198095081251],"translation_xyz_m":[0.09932061796368609,0.083078517947714,0.9912828914653731],"reprojection_error_px":0.1977393484935737}]}]},{"stamp_ns":1789379982242066097,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05791364321466741}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02856367673157794,-0.3412425425736706,-0.15104379838860468,0.927320556483988],"translation_xyz_m":[0.06699703589394841,0.10177073386694328,1.0194061765304723],"reprojection_error_px":0.15547434309780336},{"quaternion_xyzw":[-0.13010428041880331,0.38435186914431796,-0.18192847890263053,0.8956832841279052],"translation_xyz_m":[0.06693406449811595,0.10176745840389338,1.0193970952458091],"reprojection_error_px":0.20604663705664886}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.012231762526933134,0.3209419627851865,-0.23526013805865253,0.9173327138774133],"translation_xyz_m":[0.09933402297879093,0.08334663863013196,0.9911198957274249],"reprojection_error_px":0.14339594214499277},{"quaternion_xyzw":[-0.08776855477348056,-0.24665814076028592,-0.20771894395190665,0.9425016088650238],"translation_xyz_m":[0.09938703928352827,0.08332400287199886,0.9910992024409557],"reprojection_error_px":0.2019490784684821}]}]},{"stamp_ns":1789379982275386246,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0446765400427561}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.028288731041961918,-0.34102418408591045,-0.15011122612770994,0.9275607114119323],"translation_xyz_m":[0.06700670373489338,0.10180161505497362,1.0191757436914735],"reprojection_error_px":0.16156765449309182},{"quaternion_xyzw":[-0.1298399131658481,0.38425822390054604,-0.18101254993283727,0.8959473595478114],"translation_xyz_m":[0.0669432065794637,0.10179752960175655,1.0191588155002858],"reprojection_error_px":0.23122218418556306}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.010563541192700615,0.3211611891973638,-0.23265470350643083,0.9179410063218334],"translation_xyz_m":[0.09939210873503032,0.08358099278812146,0.9909673570406439],"reprojection_error_px":0.13143949716449493},{"quaternion_xyzw":[-0.08945378529610436,-0.2465833333146398,-0.20486764816300615,0.9429867055074522],"translation_xyz_m":[0.0994446821260386,0.0835578301495712,0.9909425084276833],"reprojection_error_px":0.20576518866124333}]}]},{"stamp_ns":1789379982308670593,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10740086154433619}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.028147132948441712,-0.34126378165842636,-0.1496998438406937,0.9275433828069954],"translation_xyz_m":[0.06699722561197728,0.10182523130577564,1.018999138670148],"reprojection_error_px":0.14716087952215637},{"quaternion_xyzw":[-0.12971501104777391,0.38452853834641476,-0.1806525353358922,0.8959221397994136],"translation_xyz_m":[0.06693390954928226,0.10182144658967238,1.0189851805652022],"reprojection_error_px":0.21140711683010338}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.010561840810249157,0.32166607453567236,-0.23184311798385027,0.9179695815521539],"translation_xyz_m":[0.09941233570972603,0.08368253368105025,0.9909949958985567],"reprojection_error_px":0.13706102886888413},{"quaternion_xyzw":[-0.08947051888621899,-0.24701576826318505,-0.20398720545305243,0.9430628062282137],"translation_xyz_m":[0.09946541808446088,0.08365978332043598,0.9909745571264259],"reprojection_error_px":0.19600597131626596}]}]},{"stamp_ns":1789379982342134884,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07194002742553676}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.027908631045195236,-0.34099745082560845,-0.14902880431949517,0.9277565749304725],"translation_xyz_m":[0.06701720888653105,0.10185575341251824,1.019105614577633],"reprojection_error_px":0.15931728186343017},{"quaternion_xyzw":[-0.12947107200516628,0.38435163065330924,-0.17997988827328606,0.8961686813011882],"translation_xyz_m":[0.06695410012491156,0.10185225242328828,1.0190946500656786],"reprojection_error_px":0.21468708816952978}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.00997510019188516,0.3227901542607621,-0.22947566777181966,0.9181764163760431],"translation_xyz_m":[0.09942234886101348,0.08382499684412292,0.990598605548995],"reprojection_error_px":0.15547596449932022},{"quaternion_xyzw":[-0.09001122326633126,-0.24789895836534426,-0.20142511614891206,0.943330275519594],"translation_xyz_m":[0.09947581789957136,0.08380245687427822,0.9905805355881533],"reprojection_error_px":0.20280848366954657}]}]},{"stamp_ns":1789379982375421676,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05418703343679147}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.0275057992197345,-0.3409241697500718,-0.1478697086525413,0.9279809754259624],"translation_xyz_m":[0.06704793966450413,0.10187959959555853,1.0191734018412222],"reprojection_error_px":0.15952112625745102},{"quaternion_xyzw":[-0.1290367649080503,0.3844282420807149,-0.1788525037853657,0.8964241305779879],"translation_xyz_m":[0.06698488858570902,0.1018761682485065,1.019163233294158],"reprojection_error_px":0.2128451150831439}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.008695209324092925,0.3233985668815223,-0.2269515487472666,0.9186026098346362],"translation_xyz_m":[0.09947995396903937,0.08401115942737555,0.9904890866234018],"reprojection_error_px":0.14732766851764928},{"quaternion_xyzw":[-0.09125827892804109,-0.2482261459969026,-0.19867638969948972,0.9437072634808915],"translation_xyz_m":[0.09953286311233644,0.08398801249935449,0.9904650682766732],"reprojection_error_px":0.21403023985207473}]}]},{"stamp_ns":1789379982409139829,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13131586227422015}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026929155018668615,-0.3408757145289986,-0.14738628028918427,0.9280925881811809],"translation_xyz_m":[0.06707099112598812,0.1018934348180045,1.019244745901942],"reprojection_error_px":0.15777872878178556},{"quaternion_xyzw":[-0.12845454962233863,0.38444905979811406,-0.17840767084711703,0.8965874480967123],"translation_xyz_m":[0.0670081478484946,0.10189037595342144,1.019237678685576],"reprojection_error_px":0.20286493045843199}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.007382912962463798,0.32356283363877786,-0.22569566599674995,0.9188656330683592],"translation_xyz_m":[0.09949374830314184,0.08408581924847348,0.9902242076654111],"reprojection_error_px":0.13693078615922552},{"quaternion_xyzw":[-0.09255169003990919,-0.24824084451430573,-0.19725371869511474,0.9438758595527958],"translation_xyz_m":[0.09954628628997417,0.08406219547577763,0.9901967428313418],"reprojection_error_px":0.21658952828044836}]}]},{"stamp_ns":1789379982475568207,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.047108419273631436}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026587705189007826,-0.34094681250182723,-0.14560973567585103,0.9283567039958484],"translation_xyz_m":[0.06707184748079208,0.10196556338129316,1.019023903152882],"reprojection_error_px":0.15483646890422018},{"quaternion_xyzw":[-0.12811040487454023,0.3847119075346317,-0.1767015125273882,0.8968617774409164],"translation_xyz_m":[0.06700862444697768,0.10196188805237866,1.0190112516361973],"reprojection_error_px":0.21499991419262057}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.00656137391705641,0.3248296605261852,-0.2212163314788965,0.919513988312186],"translation_xyz_m":[0.09955428201444763,0.08446338536781531,0.9899202391258122],"reprojection_error_px":0.15227902815984132},{"quaternion_xyzw":[-0.09336703807191889,-0.2490482888031121,-0.19248349052820457,0.9445674417001664],"translation_xyz_m":[0.09960710935660369,0.08443994146328283,0.989893868520394],"reprojection_error_px":0.22374732393020227}]}]},{"stamp_ns":1789379982508856376,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0687273507317112}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02631942319651721,-0.3408770789670586,-0.14487407118866263,0.9285050395634429],"translation_xyz_m":[0.06709240181712826,0.10201140284073626,1.018941213031588],"reprojection_error_px":0.16310536840392625},{"quaternion_xyzw":[-0.12786140929897508,0.3847368188514276,-0.17599757961684084,0.8970250231744806],"translation_xyz_m":[0.06702954100239479,0.102008275402065,1.018934123544416],"reprojection_error_px":0.20822778201036002}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0051909819468425695,0.3254761552503873,-0.21973755296031783,0.919648701346805],"translation_xyz_m":[0.09958305122386023,0.08461568605229798,0.9896904135450303],"reprojection_error_px":0.14395601481413617},{"quaternion_xyzw":[-0.09475551280842985,-0.24953710836982076,-0.19076710233163344,0.9446483668576535],"translation_xyz_m":[0.09963566935813765,0.08459185440342398,0.989661721194208],"reprojection_error_px":0.22423241111610345}]}]},{"stamp_ns":1789379982542233850,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06524992840576463}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.026398981178997923,-0.3408784352178038,-0.14485268297721166,0.9285056200317131],"translation_xyz_m":[0.06711663567316649,0.10202502261650068,1.0192085951730037],"reprojection_error_px":0.161049755215443},{"quaternion_xyzw":[-0.1279294194742321,0.38474825425998993,-0.17596731894954182,0.8970163583450339],"translation_xyz_m":[0.06705382768413887,0.10202193772128437,1.019202060168812],"reprojection_error_px":0.20469087488067278}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0053148336992231045,0.32544863443875083,-0.2195782930122099,0.9196957715041132],"translation_xyz_m":[0.09962237885621318,0.08463842241702786,0.9900065072718683],"reprojection_error_px":0.14614395174468903},{"quaternion_xyzw":[-0.09461676210409863,-0.24948196007736445,-0.19062600520858164,0.9447053223429571],"translation_xyz_m":[0.0996754360570564,0.08461501496955183,0.9899822836749342],"reprojection_error_px":0.21300098858741104}]}]},{"stamp_ns":1789379982575596583,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09242920736098265}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02605187533560504,-0.34063138014748984,-0.14401027271226083,0.928737101662066],"translation_xyz_m":[0.06713205797699778,0.10203760494333772,1.0191840006062984],"reprojection_error_px":0.1710467504868714},{"quaternion_xyzw":[-0.12756340938335342,0.3846148634360131,-0.1751314007328802,0.8972892375861483],"translation_xyz_m":[0.06706917803042609,0.10203440060661473,1.0191763364009998],"reprojection_error_px":0.21743103680880813}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.004453912528572433,0.32614082055210497,-0.21713931390267552,0.920034154904405],"translation_xyz_m":[0.09964342581935094,0.08478879386160675,0.9896735281484198],"reprojection_error_px":0.14178828627034917},{"quaternion_xyzw":[-0.09543146260632435,-0.2499131773086112,-0.18800079782987142,0.9450354172027968],"translation_xyz_m":[0.09969605543971104,0.08476492227235537,0.9896442267026053],"reprojection_error_px":0.22420388781043582}]}]},{"stamp_ns":1789379982608840316,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11213901487704211}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025973527628631286,-0.34040509126556023,-0.14395691816188463,0.928830531053078],"translation_xyz_m":[0.06714261057040465,0.10206581790252894,1.0193772448834073],"reprojection_error_px":0.17438870031926437},{"quaternion_xyzw":[-0.12749600919777285,0.38439980178261396,-0.17506276190979708,0.8974043622696751],"translation_xyz_m":[0.0670794646835258,0.10206218574882543,1.0193653090701909],"reprojection_error_px":0.23094613105851708}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0037641083472339373,0.32605359636095504,-0.21654543154507092,0.9202081068244274],"translation_xyz_m":[0.09964301675708023,0.08483676143197165,0.9895852379167226],"reprojection_error_px":0.14138378516625086},{"quaternion_xyzw":[-0.09612256926253845,-0.24976140549026296,-0.18733170768375895,0.9451383619884359],"translation_xyz_m":[0.0996957054319141,0.08481287021760157,0.9895568628667182],"reprojection_error_px":0.22128983901734678}]}]},{"stamp_ns":1789379982642264563,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06068029610720012}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025647017721649682,-0.3404423296429083,-0.14339273386488893,0.9289132222893848],"translation_xyz_m":[0.06712765798802124,0.10207074724682837,1.0191410719803338],"reprojection_error_px":0.1723151464715435},{"quaternion_xyzw":[-0.1271695005273183,0.38449373135050585,-0.1745368519125856,0.8975128834798093],"translation_xyz_m":[0.06706448142420848,0.1020671090161378,1.0191288967959473],"reprojection_error_px":0.22966038559580557}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0036465457045133853,0.3266213455624939,-0.21491044861639794,0.9203905140769701],"translation_xyz_m":[0.0996538421341954,0.08495799760380043,0.9894444879574981],"reprojection_error_px":0.14703262233722553},{"quaternion_xyzw":[-0.0962201699809809,-0.25016868718197216,-0.18560410503484778,0.9453615303346614],"translation_xyz_m":[0.09970666138426909,0.0849342095006085,0.9894165173202915],"reprojection_error_px":0.2238568755367228}]}]},{"stamp_ns":1789379982675494180,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06848267316923796}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02527578153690497,-0.3407106612215891,-0.14241079403510018,0.9289760739329826],"translation_xyz_m":[0.06712992918304914,0.10212138370755619,1.0190836392949163],"reprojection_error_px":0.15125110844878975},{"quaternion_xyzw":[-0.12680337877845071,0.38485643764378485,-0.1736335647833215,0.8975844309674385],"translation_xyz_m":[0.06706678419931562,0.10211781146206361,1.0190720540352722],"reprojection_error_px":0.20899300495643813}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0032741644298626116,0.3270514837615615,-0.21381690410920243,0.9204938556741178],"translation_xyz_m":[0.0996565038937069,0.08507718985212566,0.9892075140857589],"reprojection_error_px":0.14053538915548902},{"quaternion_xyzw":[-0.096620452195557,-0.2504945479943032,-0.1843983254807858,0.9454703735192013],"translation_xyz_m":[0.09970919201129332,0.08505322442830353,0.9891776975638558],"reprojection_error_px":0.22453827285027117}]}]},{"stamp_ns":1789379982709062485,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06770277847191593}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025196270347664185,-0.3406435088871513,-0.14224205833867468,0.9290287103492516],"translation_xyz_m":[0.06714567475668871,0.10214350384407986,1.0192533642150048],"reprojection_error_px":0.1529302298738057},{"quaternion_xyzw":[-0.12672288086375777,0.38481327149720157,-0.17346567389551498,0.8976467665653264],"translation_xyz_m":[0.06708252398835086,0.10213990535122583,1.0192415184510537],"reprojection_error_px":0.2112151513123014}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0030420587780297975,0.32707154684127254,-0.21371670029357515,0.9205107936036787],"translation_xyz_m":[0.09966251767557317,0.08509302338285685,0.9891798164616571],"reprojection_error_px":0.14345848215249818},{"quaternion_xyzw":[-0.09685986596716413,-0.2504981482682067,-0.1842679795625124,0.9454703357519855],"translation_xyz_m":[0.0997154848989741,0.08506926367739423,0.9891528570824106],"reprojection_error_px":0.21846650312966756}]}]},{"stamp_ns":1789379982742200369,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05920782014478434}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02535517822613583,-0.34076267978448044,-0.14235006548349896,0.9289641380914895],"translation_xyz_m":[0.06714378839255793,0.10209145600623173,1.01905331343669],"reprojection_error_px":0.1559433227462049},{"quaternion_xyzw":[-0.12685404454423843,0.3849313798737169,-0.17356390881733547,0.8975586074052949],"translation_xyz_m":[0.06708078224858707,0.10208808665514352,1.0190438779608209],"reprojection_error_px":0.20762857649622404}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0030489585742973107,0.32714551284552845,-0.21346876481669932,0.9205420162731743],"translation_xyz_m":[0.099681059348454,0.08507507810531095,0.9892307506696875],"reprojection_error_px":0.14550107280277544},{"quaternion_xyzw":[-0.09681423718380555,-0.25053018214376993,-0.1840236784992668,0.9455141019916762],"translation_xyz_m":[0.09973399929498208,0.08505130797184175,0.9892034252532205],"reprojection_error_px":0.2208680131802546}]}]},{"stamp_ns":1789379982775873600,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10803211441442653}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02489813022287672,-0.3407267301205345,-0.1413348208844367,0.929144685664368],"translation_xyz_m":[0.06715340938314622,0.10210657861230646,1.0188770122541213],"reprojection_error_px":0.15912038752394184},{"quaternion_xyzw":[-0.1263817155215297,0.38502205779494075,-0.17259100632013852,0.8977739256241442],"translation_xyz_m":[0.06709029583934245,0.1021030698532089,1.0188660994679168],"reprojection_error_px":0.21453326371228917}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.00209583001697482,0.32777472020853854,-0.2112076105884247,0.9208423782158827],"translation_xyz_m":[0.09971531459513933,0.08525657191050112,0.9890296696367928],"reprojection_error_px":0.15150092881286675},{"quaternion_xyzw":[-0.09775393872707722,-0.2509169107874202,-0.18156311084043786,0.9457905202137434],"translation_xyz_m":[0.0997682716793262,0.08523270869062043,0.9890019171477442],"reprojection_error_px":0.22596995015870733}]}]},{"stamp_ns":1789379982809006713,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08573021214474931}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.025013551374450896,-0.34066200094780535,-0.14114598026890338,0.9291940247396158],"translation_xyz_m":[0.06715145807068564,0.10211865589554134,1.0188956753053342],"reprojection_error_px":0.15912003058625274},{"quaternion_xyzw":[-0.1264987955190791,0.38497429160416075,-0.1723933421993961,0.8978158971088777],"translation_xyz_m":[0.06708851361885894,0.10211536561075923,1.0188872887982985],"reprojection_error_px":0.20788433563569733}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016027826952791904,0.327797006739562,-0.21074651969469516,0.9209410718915672],"translation_xyz_m":[0.09972195509705542,0.0852989669391369,0.9890455787957323],"reprojection_error_px":0.14868289555369488},{"quaternion_xyzw":[-0.09824267654505539,-0.2508930373649604,-0.18104093782822211,0.9458463084123795],"translation_xyz_m":[0.09977486017706545,0.0852750085813294,0.9890174960888678],"reprojection_error_px":0.22485919623221265}]}]},{"stamp_ns":1789379982842281982,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03307274124220894}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02488798172933703,-0.3406381304940013,-0.14115447344057616,0.9292048574166547],"translation_xyz_m":[0.0671562752210324,0.1021413507587221,1.018987901908499],"reprojection_error_px":0.15364288083478794},{"quaternion_xyzw":[-0.12638739430560747,0.3849489014588046,-0.17241165736956696,0.8978389556203148],"translation_xyz_m":[0.06709297062218948,0.10213752335160951,1.0189739600864778],"reprojection_error_px":0.2173414921605076}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016652863329969826,0.3277846483836413,-0.210789025670993,0.9209356317148963],"translation_xyz_m":[0.09971745993054053,0.08530383069783731,0.9890079311861389],"reprojection_error_px":0.14302571018794394},{"quaternion_xyzw":[-0.09819090095578159,-0.25088834875002375,-0.18108784981542167,0.9458439480591952],"translation_xyz_m":[0.09977014690170895,0.08527969012910001,0.9889776501152454],"reprojection_error_px":0.22711609377807065}]}]},{"stamp_ns":1789379982875836592,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.055087205687040175}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024509786980798146,-0.34051853126363196,-0.14107679612439922,0.9292705406949526],"translation_xyz_m":[0.06716553095843357,0.10217394957724976,1.0191165789351186],"reprojection_error_px":0.15695989976412367},{"quaternion_xyzw":[-0.12603037144110818,0.3848388673902675,-0.1723533751845201,0.8979474960612654],"translation_xyz_m":[0.06710234710346172,0.10217034377038012,1.0191043351506734],"reprojection_error_px":0.21595290577601853}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0017794224508062171,0.3277416404406764,-0.21078347785153737,0.9209519945370694],"translation_xyz_m":[0.09973016134282751,0.08534377893140786,0.9891300798022074],"reprojection_error_px":0.14464876415789332},{"quaternion_xyzw":[-0.09810622396653104,-0.2508487392922533,-0.1810870865236284,0.9458633864934722],"translation_xyz_m":[0.09978283700191315,0.08531965491008717,0.9890997397955795],"reprojection_error_px":0.22832870615846879}]}]},{"stamp_ns":1789379982908958966,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.03744762850857518}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024728578883841126,-0.34064433645483566,-0.14108928578509708,0.9292167383685473],"translation_xyz_m":[0.06716441790031712,0.10215966858842299,1.0191600832313568],"reprojection_error_px":0.15665311743021446},{"quaternion_xyzw":[-0.1262254439076959,0.3849592547312515,-0.1723580494423092,0.8978675917414033],"translation_xyz_m":[0.06710114941173236,0.10215590362785605,1.0191465338532109],"reprojection_error_px":0.21899260682744276}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.001640379607734935,0.32777054487714147,-0.21081365066704172,0.920935059468429],"translation_xyz_m":[0.09973246295191175,0.08532726155165961,0.9892096725292949],"reprojection_error_px":0.14762116358012117},{"quaternion_xyzw":[-0.09822455542213446,-0.2508827187790847,-0.1811082234155622,0.9458380461481751],"translation_xyz_m":[0.09978504899183023,0.08530304432165331,0.9891785356817929],"reprojection_error_px":0.23240455662080703}]}]},{"stamp_ns":1789379982942493826,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10761236930226611}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02474492877764483,-0.3406859772463007,-0.14113164179254584,0.9291946045325828],"translation_xyz_m":[0.06716557715857563,0.10211764310827524,1.0190182898116975],"reprojection_error_px":0.15755256206755786},{"quaternion_xyzw":[-0.1262199231987224,0.3850078234408974,-0.17239453928415346,0.8978405369006324],"translation_xyz_m":[0.06710251872011117,0.10211421598653932,1.0190080774843446],"reprojection_error_px":0.21123195647412377}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0018118229460947796,0.32772286506636916,-0.21072766588306605,0.9209713849201708],"translation_xyz_m":[0.09974229555812014,0.08529742051985724,0.9892303517194988],"reprojection_error_px":0.14451185647335668},{"quaternion_xyzw":[-0.0980203184558318,-0.2508111288002405,-0.18105547592316618,0.9458883176559495],"translation_xyz_m":[0.09979495403210094,0.08527330310781246,0.989199878764348],"reprojection_error_px":0.228574741594316}]}]},{"stamp_ns":1789379982975656485,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13417345730421346}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024673489712273695,-0.3407371033283407,-0.14102118019304666,0.9291945286417641],"translation_xyz_m":[0.06718059833568078,0.10212643032619842,1.0190691384841435],"reprojection_error_px":0.15896598170771675},{"quaternion_xyzw":[-0.12614722356596733,0.3850782836378153,-0.17229567910343924,0.8978395137325137],"translation_xyz_m":[0.06711771008821879,0.10212326573649422,1.0190614765291846],"reprojection_error_px":0.20574259149359866}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.001609219722697078,0.32778075763828396,-0.21075283744191542,0.9209453984054311],"translation_xyz_m":[0.09973209629568765,0.08528871170318025,0.9889976115181442],"reprojection_error_px":0.1446464675758676},{"quaternion_xyzw":[-0.09823362166632571,-0.2508622197324736,-0.18105145729528796,0.9458534094120852],"translation_xyz_m":[0.0997849288052909,0.08526469146760839,0.9889688071579023],"reprojection_error_px":0.22412118310603815}]}]},{"stamp_ns":1789379983009067257,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.05760681067478545}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024708199543339235,-0.3405611204796541,-0.14100971762589126,0.9292598601188128],"translation_xyz_m":[0.0671817033949749,0.10215962172457792,1.0192240279478493],"reprojection_error_px":0.1486866055871352},{"quaternion_xyzw":[-0.12620246491527345,0.3849007999139636,-0.1722702911649234,0.897912723407377],"translation_xyz_m":[0.06711826058782391,0.10215557968572019,1.0192077610464765],"reprojection_error_px":0.21889520283868327}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016443935129631417,0.32767083751787535,-0.2107576185083507,0.920983357314885],"translation_xyz_m":[0.09974344295087467,0.08532056015828789,0.9892090837762134],"reprojection_error_px":0.14607170731055674},{"quaternion_xyzw":[-0.09821372064168479,-0.2507619857004254,-0.18106430077639526,0.9458795962434393],"translation_xyz_m":[0.09979620823483584,0.08529651024787782,0.9891798217976062],"reprojection_error_px":0.22630876314655876}]}]},{"stamp_ns":1789379983042375204,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.07499742097390241}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024723335511799692,-0.3405289859457932,-0.1411441527167065,0.9292508243557193],"translation_xyz_m":[0.06717172873331904,0.10218886293206682,1.0192524587117187],"reprojection_error_px":0.16101459862160006},{"quaternion_xyzw":[-0.12624528447255448,0.384837572404517,-0.1724078010131515,0.8979074123561134],"translation_xyz_m":[0.06710876044974663,0.10218554134597424,1.0192433557053113],"reprojection_error_px":0.21155884716557605}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0019096007331243269,0.327791347568949,-0.21077400324948564,0.9209362113839146],"translation_xyz_m":[0.09970686186625845,0.08532478036509722,0.9889440726631157],"reprojection_error_px":0.1453032275552475},{"quaternion_xyzw":[-0.09797207734142546,-0.25090288096018104,-0.18108722949074801,0.9458629032278685],"translation_xyz_m":[0.09975944872581402,0.08530058152159523,0.9889126159304239],"reprojection_error_px":0.2318065011427759}]}]},{"stamp_ns":1789379983075748984,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09557793841886494}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024591960048348856,-0.34063130332265407,-0.14107874019169098,0.9292267429231769],"translation_xyz_m":[0.06715562357754563,0.1021816122152044,1.0190776589524078],"reprojection_error_px":0.15720761186226562},{"quaternion_xyzw":[-0.12611812835693423,0.38493841240925847,-0.17236547415440537,0.8978901824115856],"translation_xyz_m":[0.06709259159816283,0.10217823336748978,1.0190677761926197],"reprojection_error_px":0.2100219420808531}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016372307384165336,0.32780391423314453,-0.2107201584419248,0.9209445847146008],"translation_xyz_m":[0.09972942898200526,0.08536098199887948,0.9892117698430403],"reprojection_error_px":0.15043001149408636},{"quaternion_xyzw":[-0.09824850475779334,-0.25091495386465923,-0.1810012091881112,0.945847492734635],"translation_xyz_m":[0.09978234236502695,0.08533704310295394,0.9891838318375953],"reprojection_error_px":0.2256412617490992}]}]},{"stamp_ns":1789379983109311222,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0386546044453378}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02478268505253859,-0.34051917328904907,-0.14117532287464146,0.9292481043055024],"translation_xyz_m":[0.0671662217286962,0.10216887967110101,1.019180464989124],"reprojection_error_px":0.15552801170666822},{"quaternion_xyzw":[-0.12629501197991255,0.3848274728103865,-0.17242995324518767,0.8979004941212737],"translation_xyz_m":[0.06710298025298708,0.10216514135047806,1.0191672791122552],"reprojection_error_px":0.21707136813490097}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0017963546224393403,0.3275355753341088,-0.2107821079937022,0.9210255821367591],"translation_xyz_m":[0.0997523928955417,0.08534943942021501,0.9893994978763382],"reprojection_error_px":0.14097043731520703},{"quaternion_xyzw":[-0.09807793922572068,-0.25063910256410465,-0.1811113433975229,0.9459172476468756],"translation_xyz_m":[0.09980489700432653,0.0853252103109591,0.9893678048844863],"reprojection_error_px":0.22956384994463527}]}]},{"stamp_ns":1789379983142367342,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.0926035107332877}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02464887159001396,-0.34073532509370796,-0.14105337069028256,0.9291909480723184],"translation_xyz_m":[0.06718992810408363,0.10215792748087604,1.0192708681940843],"reprojection_error_px":0.15336234018775985},{"quaternion_xyzw":[-0.12613421289163623,0.3850692627918617,-0.17233266386194598,0.8978381124442117],"translation_xyz_m":[0.06712673443885013,0.10215428519300644,1.0192583823073027],"reprojection_error_px":0.21329902197446313}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0019422226354547678,0.32769947628615287,-0.21073133108249908,0.9209786029616613],"translation_xyz_m":[0.09975421459023198,0.08531936914474218,0.9892594338860979],"reprojection_error_px":0.13553835510249299},{"quaternion_xyzw":[-0.09790948955854951,-0.25078409765055043,-0.18106800784708965,0.9459045642950824],"translation_xyz_m":[0.0998066638283072,0.08529509397910591,0.9892268621963507],"reprojection_error_px":0.22855063518242724}]}]},{"stamp_ns":1789379983175765310,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.15301358980087815}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02483825609203876,-0.34069917884105827,-0.1411137787457714,0.929189987042168],"translation_xyz_m":[0.06718350360174491,0.10211545538297728,1.0189657976844704],"reprojection_error_px":0.15553674570621792},{"quaternion_xyzw":[-0.12631777205707073,0.38504037561946974,-0.1723726933700898,0.8978170104123091],"translation_xyz_m":[0.06712049000096454,0.10211208532335184,1.0189563193260094],"reprojection_error_px":0.2073853871101698}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0017536474999873524,0.32776850835101085,-0.2106869747680965,0.9209645640940082],"translation_xyz_m":[0.09974566088180414,0.08529575842612573,0.9890916853005687],"reprojection_error_px":0.13703011635399764},{"quaternion_xyzw":[-0.09808544898517295,-0.2508409281350888,-0.18100177234341783,0.945883942076535],"translation_xyz_m":[0.09979795249033122,0.08527130723510473,0.9890574901740425],"reprojection_error_px":0.23383069139253107}]}]},{"stamp_ns":1789379983209089978,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.089274271910344}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024751873313168162,-0.3407500300733264,-0.14101839423712667,0.9291881264089088],"translation_xyz_m":[0.06717860185699306,0.10214333827958501,1.019121044350269],"reprojection_error_px":0.14813899114789675},{"quaternion_xyzw":[-0.1262350267048182,0.3850867689843698,-0.17229306888272208,0.897824034430459],"translation_xyz_m":[0.06711520218864786,0.10213937743697912,1.0191055695798428],"reprojection_error_px":0.2164147163202241}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.001601942920295827,0.3277142962561759,-0.21078137566788765,0.9209625320716106],"translation_xyz_m":[0.09973013639201997,0.08530581154661948,0.9890828905547303],"reprojection_error_px":0.13948640783693408},{"quaternion_xyzw":[-0.09825166024357244,-0.2508103382539283,-0.18108154312509284,0.9458595351444985],"translation_xyz_m":[0.09978265944412651,0.08528153466214752,0.9890511371130323],"reprojection_error_px":0.22882376390228004}]}]},{"stamp_ns":1789379983242444604,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04155490860530889}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024799360493849915,-0.3405123911112998,-0.14108880182874395,0.9292632851981675],"translation_xyz_m":[0.06717169629792144,0.10217418608421171,1.0193000996394552],"reprojection_error_px":0.15758573642936194},{"quaternion_xyzw":[-0.1263001069962956,0.3848295687375835,-0.1723398988255398,0.8979161682867575],"translation_xyz_m":[0.06710844266497533,0.10217041016849471,1.0192866132683729],"reprojection_error_px":0.21965571451334073}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0012787583679579719,0.32768111477034256,-0.2108201523510993,0.9209659685148857],"translation_xyz_m":[0.09971303052001583,0.08531989177666421,0.9890740142294598],"reprojection_error_px":0.13636680339301302},{"quaternion_xyzw":[-0.0985854944387738,-0.2507989034942837,-0.18108602928550663,0.9458269716443575],"translation_xyz_m":[0.09976550562199692,0.08529552640479561,0.9890419958260903],"reprojection_error_px":0.2275752490006425}]}]},{"stamp_ns":1789379983275726430,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04826347202308441}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024961916118859544,-0.34064259891323917,-0.14105537636870027,0.9292162844821401],"translation_xyz_m":[0.06714517903207742,0.10214296874999924,1.0189890245280235],"reprojection_error_px":0.15179543019707056},{"quaternion_xyzw":[-0.1264556676262681,0.3849542046374368,-0.17230981443508855,0.8978466195885011],"translation_xyz_m":[0.06708180315388307,0.10213901365864077,1.0189739919712895],"reprojection_error_px":0.21848999543490355}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0015561604594122992,0.3277595037900366,-0.2107470230719404,0.9209543844872278],"translation_xyz_m":[0.09969554502254803,0.08530694391183169,0.9889209378078457],"reprojection_error_px":0.14453356645874094},{"quaternion_xyzw":[-0.0983028491382883,-0.2508715411191976,-0.18103432481110118,0.945847024073112],"translation_xyz_m":[0.09974845485439787,0.08528297531007546,0.988892889260514],"reprojection_error_px":0.22196097555038163}]}]},{"stamp_ns":1789379983309221097,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.028703344926093496}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024848545867122148,-0.3403860911499802,-0.14110202353175688,0.9293062346047025],"translation_xyz_m":[0.06716892378547014,0.10217017730359133,1.0192362779977804],"reprojection_error_px":0.15982646844576634},{"quaternion_xyzw":[-0.1263566696816069,0.38470553724226075,-0.17233812334030532,0.8979616990081808],"translation_xyz_m":[0.06710585333784094,0.1021666698126276,1.0192256032458906],"reprojection_error_px":0.21455310031306757}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0015142362826712822,0.3277975377367312,-0.2107643883849989,0.9209369435150888],"translation_xyz_m":[0.0997273927108148,0.08532699050135037,0.9891472178657272],"reprojection_error_px":0.13714829618786994},{"quaternion_xyzw":[-0.09834560313985032,-0.2509054073252692,-0.18104417115082377,0.9458317117808596],"translation_xyz_m":[0.09978004125360028,0.08530280432201667,0.9891166982021592],"reprojection_error_px":0.22393959869269317}]}]},{"stamp_ns":1789379983342598995,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10115485677566288}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024890403202151655,-0.3405308555573873,-0.14101995443351528,0.9292645353683511],"translation_xyz_m":[0.067187414513949,0.10215852611568545,1.0191744465782646],"reprojection_error_px":0.15576557005466243},{"quaternion_xyzw":[-0.12638864259524546,0.3848753693384016,-0.17226643585073542,0.897898176954851],"translation_xyz_m":[0.06712434470252229,0.10215502348427109,1.019163875300358],"reprojection_error_px":0.2105151486658461}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016733126261470317,0.327831951128873,-0.21075597133735785,0.9209263447146782],"translation_xyz_m":[0.09973186860082375,0.0853073637389279,0.9889721670881554],"reprojection_error_px":0.13358672111670872},{"quaternion_xyzw":[-0.09818665345805466,-0.25091922735467914,-0.18104984447957503,0.9458434734354096],"translation_xyz_m":[0.09978426521773683,0.08528297119696299,0.9889389407859926],"reprojection_error_px":0.22916864410446794}]}]},{"stamp_ns":1789379983375829758,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.14304859292324476}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024851484913661397,-0.34073243635497497,-0.14110210999903183,0.9291792104147673],"translation_xyz_m":[0.06719464436508166,0.1021524699740936,1.0191598040615721],"reprojection_error_px":0.15916907033717292},{"quaternion_xyzw":[-0.12634129935594918,0.3850707087661906,-0.17236793373054882,0.8978016043362895],"translation_xyz_m":[0.06713141929892433,0.10214875846384384,1.01914693730268],"reprojection_error_px":0.21953193262739767}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0015159851847167138,0.3277059743340396,-0.21074937033657706,0.9209729632716879],"translation_xyz_m":[0.09975801835984692,0.08531771015622648,0.9891546730834958],"reprojection_error_px":0.1332655837180204},{"quaternion_xyzw":[-0.09834210676000396,-0.2507805568567608,-0.18104024952874928,0.9458659367961113],"translation_xyz_m":[0.09981044719331106,0.08529334876382672,0.9891220940853827],"reprojection_error_px":0.22717111675631932}]}]},{"stamp_ns":1789379983409158322,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.039849417219750126}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024494611167901904,-0.340539785475675,-0.14107969230403974,0.9292627125583551],"translation_xyz_m":[0.06716125054607695,0.10215606693635897,1.0190831385845356],"reprojection_error_px":0.16074687686443417},{"quaternion_xyzw":[-0.12600123634415983,0.3848588373735476,-0.1723548676203217,0.8979427394565901],"translation_xyz_m":[0.06709826703874484,0.10215277439435848,1.0190739738075925],"reprojection_error_px":0.21147197220682043}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0017892408374564153,0.3277976069066299,-0.21077521748884573,0.9209339472600855],"translation_xyz_m":[0.09972348810988735,0.08532371650147581,0.9890881022471792],"reprojection_error_px":0.1359707804184333},{"quaternion_xyzw":[-0.09807718441845586,-0.25090630371998146,-0.1810811924009351,0.9458522582352807],"translation_xyz_m":[0.09977583824516142,0.08529930893834185,0.989054437912041],"reprojection_error_px":0.2317875395753744}]}]},{"stamp_ns":1789379983442389494,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.10829475559989316}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024547107210951435,-0.3406155312558746,-0.14086948702214283,0.9292654556266313],"translation_xyz_m":[0.06714087243033727,0.10217457336724618,1.0192447268727207],"reprojection_error_px":0.14756243880270553},{"quaternion_xyzw":[-0.12604082035179243,0.3849293914746657,-0.17215286129212293,0.897945692975319],"translation_xyz_m":[0.06707748299502797,0.10217062986427211,1.0192292516566084],"reprojection_error_px":0.21589619104369523}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0013549057532754789,0.3277692276960889,-0.21078935476462327,0.9209415538044871],"translation_xyz_m":[0.09969522175449083,0.08533226363267592,0.9891133032047045],"reprojection_error_px":0.13758899925534707},{"quaternion_xyzw":[-0.0985088578176566,-0.2509081423949407,-0.18105279783529363,0.9458123457681198],"translation_xyz_m":[0.09974775978638069,0.08530795680489199,0.9890817719484765],"reprojection_error_px":0.22700849683689076}]}]},{"stamp_ns":1789379983475725637,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.060657255863363575}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02478907936403689,-0.3406844050139951,-0.14107261935664314,0.9292029669519999],"translation_xyz_m":[0.06715329431401747,0.10216308372250915,1.0191951949580176],"reprojection_error_px":0.15390344703546038},{"quaternion_xyzw":[-0.12628256422710113,0.38498778253582494,-0.17234243791443174,0.8978503245887295],"translation_xyz_m":[0.06709004951486594,0.10215934245799842,1.0191819758476868],"reprojection_error_px":0.21569301996084078}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0014413610796056475,0.32771781363652364,-0.21082259656549177,0.9209521105252536],"translation_xyz_m":[0.09970870091754298,0.08532136754454575,0.9891130736486485],"reprojection_error_px":0.1420354051309905},{"quaternion_xyzw":[-0.09842066740905148,-0.25084306240606935,-0.18110218002355652,0.945829334848638],"translation_xyz_m":[0.09976144497878013,0.08529725418129663,0.9890836071243775],"reprojection_error_px":0.22420511972536755}]}]},{"stamp_ns":1789379983509040349,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.08408892987412184}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024595161205432536,-0.3407955439829159,-0.14093879677380558,0.9291876725455516],"translation_xyz_m":[0.06717249164876878,0.10214065202609875,1.018995225298428],"reprojection_error_px":0.15468131908408048},{"quaternion_xyzw":[-0.12608123010362143,0.3851395852702164,-0.17223120868880262,0.8978348589952594],"translation_xyz_m":[0.06710919338940123,0.10213688052292211,1.0189814185692838],"reprojection_error_px":0.21790760969691225}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016608256287057941,0.3277021492126923,-0.21079513647466963,0.9209636005284025],"translation_xyz_m":[0.09974205358069069,0.08531901109089465,0.9891017816042915],"reprojection_error_px":0.13407145259081868},{"quaternion_xyzw":[-0.09820686498153082,-0.2507934582711705,-0.18109806105977252,0.9458655006073945],"translation_xyz_m":[0.09979429302094645,0.08529450114829348,0.9890672184037027],"reprojection_error_px":0.23302704443695163}]}]},{"stamp_ns":1789379983542501898,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.13288573431043185}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02466271055265799,-0.34063866322537817,-0.1410297951750294,0.9292295995592869],"translation_xyz_m":[0.06719113812570779,0.10214781967367241,1.0190825942422104],"reprojection_error_px":0.1615355579966614},{"quaternion_xyzw":[-0.1261569454642233,0.3849882944508723,-0.17230007732407285,0.8978758943200691],"translation_xyz_m":[0.06712803584926068,0.10214432290486816,1.0190716484591267],"reprojection_error_px":0.21683384524369756}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016268045901747961,0.3279049806810142,-0.21075120737623942,0.9209015179381921],"translation_xyz_m":[0.09972613240290083,0.0852972556695549,0.9888399695219815],"reprojection_error_px":0.1350281082282254},{"quaternion_xyzw":[-0.09823326846993657,-0.2509865500614278,-0.1810335205240239,0.9458238953952547],"translation_xyz_m":[0.09977856150930293,0.08527286894184379,0.9888069479750708],"reprojection_error_px":0.22951357585291993}]}]},{"stamp_ns":1789379983575737039,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.09494708244821892}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02461771611601228,-0.34074343399415036,-0.14097520895359467,0.9292006622380312],"translation_xyz_m":[0.06718237337457018,0.10214604625862335,1.0191361173970166],"reprojection_error_px":0.15586984825183584},{"quaternion_xyzw":[-0.12610043202217108,0.3850860639855333,-0.17225830916074958,0.8978499202500555],"translation_xyz_m":[0.06711927965129191,0.10214256889593314,1.0191252748937534],"reprojection_error_px":0.2113083291657983}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0016457426488589789,0.32781111238192895,-0.21076071101620147,0.9209327276310693],"translation_xyz_m":[0.0997212610818474,0.08529247654488599,0.9889087858512917],"reprojection_error_px":0.13001852021902502},{"quaternion_xyzw":[-0.09820483862188593,-0.2509012014648093,-0.18105709481687954,0.945844979471391],"translation_xyz_m":[0.0997735434991021,0.08526798030123527,0.9888744241035735],"reprojection_error_px":0.23011432566031204}]}]},{"stamp_ns":1789379983609178597,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06668026006206398}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02472682143066488,-0.3406298630242571,-0.1411477628875307,0.929213210059918],"translation_xyz_m":[0.06717181350890156,0.102164119196861,1.0193372412096606],"reprojection_error_px":0.15792373508917734},{"quaternion_xyzw":[-0.12621582609875498,0.38493610100670084,-0.1724112233218593,0.897868661585105],"translation_xyz_m":[0.06710879229383739,0.10216071846821652,1.0193272811358778],"reprojection_error_px":0.2108817501624193}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0021634509938150246,0.32785084326280056,-0.21067353939253336,0.9209374592508932],"translation_xyz_m":[0.09973275418442185,0.08532079043907179,0.989267350216717],"reprojection_error_px":0.15391269378034714},{"quaternion_xyzw":[-0.09768219866549266,-0.2509522331150769,-0.18101827480157182,0.9458929902194508],"translation_xyz_m":[0.09978552758399584,0.08529681175813693,0.9892376875795273],"reprojection_error_px":0.2327173181727559}]}]},{"stamp_ns":1789379983642451298,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06257968788138206}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024662167398952196,-0.34070638342920107,-0.14107155154214576,0.9291984476608169],"translation_xyz_m":[0.06715341581836179,0.10216603436649001,1.0191899517797216],"reprojection_error_px":0.15612783852910198},{"quaternion_xyzw":[-0.12615791605341686,0.385009540552276,-0.17235248308851117,0.8978565895921291],"translation_xyz_m":[0.06709012197310155,0.1021622416011137,1.0191760010034443],"reprojection_error_px":0.21952469576108646}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.001478078570237061,0.32775774134404273,-0.21075346213645,0.9209536668423074],"translation_xyz_m":[0.09970265436692552,0.08532102793616458,0.9890335974602245],"reprojection_error_px":0.13886030180138925},{"quaternion_xyzw":[-0.0983824268351671,-0.2508760489162016,-0.181032137588831,0.945837973085368],"translation_xyz_m":[0.09975544949543684,0.08529695725807625,0.9890045178419777],"reprojection_error_px":0.22105259328400106}]}]},{"stamp_ns":1789379983675821504,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.04719124726188249}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024639146293661433,-0.34063740152079325,-0.14108059043081136,0.9292229765555602],"translation_xyz_m":[0.06716115276965588,0.10217260346129751,1.0192024836626858],"reprojection_error_px":0.16388852856139582},{"quaternion_xyzw":[-0.12614194825427125,0.3849458042858794,-0.17235703381500395,0.8978852875216342],"translation_xyz_m":[0.06709809747593211,0.10216917225588308,1.019192129458627],"reprojection_error_px":0.21748541198568946}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0015472160366362861,0.3278405965897675,-0.2107580865701797,0.9209230034565744],"translation_xyz_m":[0.09970730589175578,0.0853256346989211,0.9890355809014498],"reprojection_error_px":0.13733107032453665},{"quaternion_xyzw":[-0.09831867475868017,-0.25096001570395815,-0.18103411583199555,0.9458219481575237],"translation_xyz_m":[0.09975963979506641,0.08530117096526711,0.989001808337926],"reprojection_error_px":0.23290479306892242}]}]},{"stamp_ns":1789379983709387256,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.045524100902507164}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.02471127579433285,-0.3406951974126015,-0.1411001901594931,0.9291968960589352],"translation_xyz_m":[0.06716144419241686,0.10215938153893575,1.0190296128429506],"reprojection_error_px":0.15874647756066904},{"quaternion_xyzw":[-0.12621938419133677,0.38500920238488595,-0.1723790589921042,0.8978429935987196],"translation_xyz_m":[0.06709833259998141,0.10215587308481915,1.0190185788378885],"reprojection_error_px":0.2144829844727361}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0019569938231224497,0.32795458543503275,-0.21072368505506647,0.9208895094549212],"translation_xyz_m":[0.0997062711085536,0.08531609690664343,0.9888966207788542],"reprojection_error_px":0.14524227100878825},{"quaternion_xyzw":[-0.09791190849458703,-0.25105968116150035,-0.18102995928445528,0.9458384896542866],"translation_xyz_m":[0.0997590317660741,0.08529203834387793,0.9888666552654249],"reprojection_error_px":0.2277431858834011}]}]},{"stamp_ns":1789379983742521013,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.091581484318777}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024820724551827152,-0.3407121391932917,-0.14097319479033849,0.9292070426926706],"translation_xyz_m":[0.06717327191544177,0.10214643872882806,1.0189303839249537],"reprojection_error_px":0.15662118759099702},{"quaternion_xyzw":[-0.12632291078963356,0.38505487416946466,-0.17224682976247377,0.8978342250798581],"translation_xyz_m":[0.06711034667531672,0.10214319780960993,1.0189222742917354],"reprojection_error_px":0.20470371687888186}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0020426577213665934,0.3279842474345056,-0.2106643454666906,0.9208923360161844],"translation_xyz_m":[0.09972916319218485,0.08532221851431444,0.9889691639825915],"reprojection_error_px":0.14599857731453597},{"quaternion_xyzw":[-0.09782368501013636,-0.25106829906759764,-0.18097722462443516,0.9458554223672757],"translation_xyz_m":[0.09978209619469099,0.0852983258718055,0.988940868753361],"reprojection_error_px":0.22364178019679815}]}]},{"stamp_ns":1789379983775940730,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.11508663759139856}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024903116463402413,-0.3406037739418631,-0.14115881839257682,0.9292163859709868],"translation_xyz_m":[0.067182269623415,0.10214035386510201,1.0190131706101537],"reprojection_error_px":0.15940536917989234},{"quaternion_xyzw":[-0.12640317248141494,0.3849358256374163,-0.17240948511857093,0.8978427577079539],"translation_xyz_m":[0.06711923303149608,0.10213692828648406,1.0190032719275728],"reprojection_error_px":0.21216063684836578}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.001583487712965245,0.3278148097607588,-0.21078776545605443,0.9209253286788369],"translation_xyz_m":[0.09973950064779259,0.08531291104644803,0.9890466657830697],"reprojection_error_px":0.133260261576966},{"quaternion_xyzw":[-0.0982778927427452,-0.2509047997156485,-0.18107418782503207,0.945833164876978],"translation_xyz_m":[0.09979188459130395,0.08528850027870116,0.9890134310238932],"reprojection_error_px":0.22897622212814922}]}]},{"stamp_ns":1789379983809683228,"direction":"increasing","roles":[{"role":"front_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.05140881258498139,0.07233102729244478,0.05368859617193665,-0.9946069028116127],"translation_xyz_m":[0.01947103242155611,0.06865579974886113,1.0453629613649875],"reprojection_error_px":0.06738700553712068}]},{"role":"thumb_pitch","frozen":false,"candidates":[{"quaternion_xyzw":[0.024921984924120878,-0.3406998551626559,-0.1411270287105558,0.9291854847805776],"translation_xyz_m":[0.06714526502506334,0.1021347717189194,1.0190146113101635],"reprojection_error_px":0.15855931849500823},{"quaternion_xyzw":[-0.12640582549503918,0.38500178765434206,-0.1723853114643265,0.8978187429404811],"translation_xyz_m":[0.06708202784130643,0.10213104147158723,1.0190016753563753],"reprojection_error_px":0.21917147102692042}]},{"role":"thumb_ip","frozen":false,"candidates":[{"quaternion_xyzw":[-0.0018988389109724545,0.32796753163862913,-0.21072105142958675,0.9208856232377962],"translation_xyz_m":[0.09970368228820628,0.08530565067444842,0.9890463351134585],"reprojection_error_px":0.15767288029394486},{"quaternion_xyzw":[-0.09794618614210304,-0.2510825580432756,-0.18102812038627294,0.9458292199419908],"translation_xyz_m":[0.09975688293523159,0.08528197091722774,0.9890208198609118],"reprojection_error_px":0.22414317613527718}]}]}],"yaw_retry_frames":[{"stamp_ns":1789379937778301151,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05818500580587801}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15183433358614357,0.10658744043832057,-0.43051401054853505,0.8833137264905799],"translation_xyz_m":[0.09662420313175064,0.03430352987343674,0.6644897280057548],"reprojection_error_px":0.11918844975472014},{"quaternion_xyzw":[0.04313051683174511,-0.00039456978187015493,-0.4418571107016943,0.896047932063271],"translation_xyz_m":[0.09665350227525424,0.03432565907817043,0.6644567458394233],"reprojection_error_px":0.3306283781380559}]}]},{"stamp_ns":1789379937811610635,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05376471862851026}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15192807530275393,0.10651779990369044,-0.43052567348004656,0.8833003241893612],"translation_xyz_m":[0.09661946054923906,0.03430129534326216,0.6644562037204753],"reprojection_error_px":0.10844058287424699},{"quaternion_xyzw":[0.04322105862293663,-0.0003253010405147692,-0.4418859838597389,0.8960293586367896],"translation_xyz_m":[0.09664892879731163,0.03432352872257317,0.6644244299534968],"reprojection_error_px":0.3194654755697112}]}]},{"stamp_ns":1789379937844878282,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05385565390085307}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15194299197618671,0.10640993963120056,-0.43058059265142573,0.8832839889689815],"translation_xyz_m":[0.0966242859444302,0.03430733356315548,0.6645019871587292],"reprojection_error_px":0.11053973008776133},{"quaternion_xyzw":[0.04322457701519445,-0.00022930704111637757,-0.4419468250833955,0.8959992115838301],"translation_xyz_m":[0.09665373336120085,0.03432958731374803,0.6644702942067505],"reprojection_error_px":0.3196775431312026}]}]},{"stamp_ns":1789379937878054187,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05330141360126375}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15218933463284387,0.10638622213707766,-0.43054294676256244,0.8832627860135521],"translation_xyz_m":[0.09662533192035415,0.03430814863903252,0.6645051267952143],"reprojection_error_px":0.10744958905261301},{"quaternion_xyzw":[0.043473006518959724,-0.00020586605465244248,-0.44194626179046026,0.8959874759240822],"translation_xyz_m":[0.09665483891915633,0.034330497026806545,0.6644736463609027],"reprojection_error_px":0.31720776200932166}]}]},{"stamp_ns":1789379937911876282,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0607986687072998}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1520171172463126,0.10670873305615941,-0.4305528321672489,0.8832487198215637],"translation_xyz_m":[0.09662147716984218,0.034310112802477706,0.6644736617900197],"reprojection_error_px":0.109510283663962},{"quaternion_xyzw":[0.04329645747114086,-0.0005291362214245348,-0.44191410731274217,0.8960117513421858],"translation_xyz_m":[0.09665106395189653,0.03433235187368485,0.6644421760749462],"reprojection_error_px":0.31789072602312696}]}]},{"stamp_ns":1789379937944683867,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05008668711004953}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15180644697419757,0.10656517369099755,-0.43057471627275906,0.8832916166928737],"translation_xyz_m":[0.09662180212671857,0.03431072674611209,0.6644642029423194],"reprojection_error_px":0.1160723569371474},{"quaternion_xyzw":[0.043082232722199215,-0.0003820142470097261,-0.44191276656615025,0.896022813344971],"translation_xyz_m":[0.09665117443917637,0.034332874103107015,0.6644317829040514],"reprojection_error_px":0.3261223012029511}]}]},{"stamp_ns":1789379937978302446,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05361348695674729}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15195844529837688,0.10645689328844289,-0.4305549913606828,0.8832881524102287],"translation_xyz_m":[0.09662793551646136,0.034312666946816534,0.6645032643272067],"reprojection_error_px":0.1158133051328607},{"quaternion_xyzw":[0.0432347031589164,-0.00027392017722636285,-0.44192065810095765,0.8960116167517659],"translation_xyz_m":[0.09665732171863632,0.03433489025682396,0.664471040634775],"reprojection_error_px":0.32478332811103666}]}]},{"stamp_ns":1789379938011759379,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0706665680467489}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1519921728205412,0.10661645610872758,-0.430568545509278,0.8832564963282927],"translation_xyz_m":[0.09662084216397104,0.034314090739205945,0.6644613622730895],"reprojection_error_px":0.10984330597143442},{"quaternion_xyzw":[0.0432627336813147,-0.00043906330033361893,-0.44193005988083733,0.8960055609601747],"translation_xyz_m":[0.09665037703761455,0.03433632884953002,0.6644297475488585],"reprojection_error_px":0.3188686049851962}]}]},{"stamp_ns":1789379938044687589,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05287338013301857}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1522042126599244,0.10669384632985998,-0.430479932907859,0.8832538299761614],"translation_xyz_m":[0.09660845190535394,0.03431362753949632,0.6644266172738107],"reprojection_error_px":0.11074811478210247},{"quaternion_xyzw":[0.04348805191377912,-0.0005221741750197422,-0.4418677278356721,0.8960253499607116],"translation_xyz_m":[0.09663803128325088,0.034335920845276376,0.6643949025155873],"reprojection_error_px":0.3197129365314576}]}]},{"stamp_ns":1789379938078046568,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.057303989237281444}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1520433951446174,0.10625031217249474,-0.43045579729776784,0.8833467516943095],"translation_xyz_m":[0.09662147358771887,0.03431155226056188,0.664528777075492],"reprojection_error_px":0.11822971581902278},{"quaternion_xyzw":[0.04333881833861351,-6.893619005157535e-05,-0.4418443097356237,0.8960442779389111],"translation_xyz_m":[0.09665083438518392,0.03433385406910674,0.6644967650629742],"reprojection_error_px":0.32437200400073823}]}]},{"stamp_ns":1789379938111673040,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04957980913563369}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1518439149147426,0.10656973799305952,-0.4305241349514595,0.8833092808703826],"translation_xyz_m":[0.09661955122344945,0.03431611014948748,0.6645106570864582],"reprojection_error_px":0.11331099677620048},{"quaternion_xyzw":[0.043125777362312684,-0.00039481605539479225,-0.44186607799720995,0.8960437380856708],"translation_xyz_m":[0.09664904071804219,0.034338308691007575,0.6644790040430134],"reprojection_error_px":0.32035380574178185}]}]},{"stamp_ns":1789379938144994905,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07097268765818686}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15194403964574335,0.10607795113556957,-0.4304118708193708,0.8834059647505115],"translation_xyz_m":[0.09662319305919624,0.03431350544429531,0.664529067315999],"reprojection_error_px":0.11874694827572833},{"quaternion_xyzw":[0.04324160881450569,0.00011328916505236359,-0.4417953968431201,0.896073087287487],"translation_xyz_m":[0.09665241961385558,0.0343357860940156,0.6644966408810424],"reprojection_error_px":0.32721407440367906}]}]},{"stamp_ns":1789379938178204155,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.060156273381609525}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15206692802119257,0.1057897642209492,-0.43045471918380235,0.8833985000670237],"translation_xyz_m":[0.0966256860638076,0.03431385795339135,0.6645501399000976],"reprojection_error_px":0.11394648438569893},{"quaternion_xyzw":[0.04335778731144376,0.00039566602588052177,-0.4418702609726579,0.8960304783855328],"translation_xyz_m":[0.09665492821819074,0.03433625428687887,0.6645183345911594],"reprojection_error_px":0.3217284175875516}]}]},{"stamp_ns":1789379938211439466,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06749423433881478}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1519272497830295,0.1062270677714626,-0.43052753675208105,0.8833345690870631],"translation_xyz_m":[0.09661846665241128,0.034323296534312625,0.6645069076627239],"reprojection_error_px":0.11613581824592076},{"quaternion_xyzw":[0.04319836864755951,-5.857665050905077e-05,-0.44189746538886304,0.8960248476453453],"translation_xyz_m":[0.0966477742251731,0.03434554695718904,0.6644746805686966],"reprojection_error_px":0.32501648971228525}]}]},{"stamp_ns":1789379938244917956,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07766175379052183}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15210817765136664,0.10624183526912104,-0.430486971691301,0.8833214261718564],"translation_xyz_m":[0.09661839149151895,0.03431711516630104,0.6644955380765852],"reprojection_error_px":0.11510609692118937},{"quaternion_xyzw":[0.043388444611035026,-6.740662208951691e-05,-0.4418837607995131,0.8960224217463876],"translation_xyz_m":[0.09664781125900947,0.0343394532418151,0.6644638661797528],"reprojection_error_px":0.32113838327510746}]}]},{"stamp_ns":1789379938278160670,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05864155012971289}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15236466888934624,0.10616019869252832,-0.43047573737602307,0.8832925106771664],"translation_xyz_m":[0.09661239743476918,0.03431608197860777,0.6644422971543312],"reprojection_error_px":0.1151832519602028},{"quaternion_xyzw":[0.04364197842550925,1.0939361756642427e-05,-0.4419136118149422,0.8959953891020368],"translation_xyz_m":[0.09664180540058057,0.034338507057138395,0.6644104409309981],"reprojection_error_px":0.32227700516641966}]}]},{"stamp_ns":1789379938311708245,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05010127423755979}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15193381440947154,0.10657249694780457,-0.43048002041330224,0.8833149896601221],"translation_xyz_m":[0.09661790223227389,0.03431833007773947,0.6644794931258783],"reprojection_error_px":0.10806872352419858},{"quaternion_xyzw":[0.0432135690182405,-0.00039176386839339,-0.4418350737876878,0.8960547982935004],"translation_xyz_m":[0.09664745625486086,0.03434057750898152,0.6644481895020301],"reprojection_error_px":0.3163345913031885}]}]},{"stamp_ns":1789379938344927447,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0636725497228372}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15238572030208322,0.10676144387205633,-0.43042289414868556,0.883242163023844],"translation_xyz_m":[0.09660124565735395,0.03430373328451879,0.6643837882195824],"reprojection_error_px":0.11514817215343122},{"quaternion_xyzw":[0.04368580816725468,-0.0005855199699892713,-0.44183504329449313,0.8960318085024057],"translation_xyz_m":[0.09663088808972504,0.03432608397096887,0.6643521574884914],"reprojection_error_px":0.3206473571605861}]}]},{"stamp_ns":1789379938378287308,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05921877604769042}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15177546633401412,0.10666655013846339,-0.43052121697120577,0.8833107814570608],"translation_xyz_m":[0.09661894492645746,0.03431440591192674,0.6645171722157133],"reprojection_error_px":0.10909453591260598},{"quaternion_xyzw":[0.04306175232879334,-0.0004898160542055221,-0.44184871949521926,0.8960553301259034],"translation_xyz_m":[0.09664851193684222,0.03433658745881172,0.6644859090578874],"reprojection_error_px":0.3164301502311905}]}]},{"stamp_ns":1789379938411735327,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06209974200776654}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1519052125187724,0.1065096603027536,-0.43050712294628257,0.8833142791579524],"translation_xyz_m":[0.09662027058526684,0.03432088248229193,0.6645092425266496],"reprojection_error_px":0.10736007817771454},{"quaternion_xyzw":[0.04318183450630562,-0.00033407954283270303,-0.44186058153605146,0.8960437735089462],"translation_xyz_m":[0.0966497862951023,0.03434312363193916,0.664477846920765],"reprojection_error_px":0.3166992197921459}]}]},{"stamp_ns":1789379938444894007,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07832646248534739}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15230859970579289,0.1064290543769499,-0.4303997938430742,0.8833068347408662],"translation_xyz_m":[0.09661115020088458,0.03430594693357615,0.6644506440020534],"reprojection_error_px":0.11481311570810837},{"quaternion_xyzw":[0.04361293999364984,-0.00024505846121763313,-0.4418185647640627,0.896043641370935],"translation_xyz_m":[0.09664059394554203,0.03432830287362942,0.6644185067449242],"reprojection_error_px":0.3238070411146386}]}]},{"stamp_ns":1789379938478658815,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09033968596486498}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15232804811582132,0.10653950183707091,-0.43037428795412114,0.8833025940036155],"translation_xyz_m":[0.09660726011127757,0.03430316992762694,0.6644191755261274],"reprojection_error_px":0.11918292774204611},{"quaternion_xyzw":[0.04363581394583556,-0.0003533269813992006,-0.4417905239522078,0.8960563173413613],"translation_xyz_m":[0.09663676767831454,0.034325524802197784,0.6643872036995099],"reprojection_error_px":0.3243081318306767}]}]},{"stamp_ns":1789379938511658023,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.13087893695597277}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15179963555437587,0.106551065650006,-0.4305711376147112,0.8832962337220623],"translation_xyz_m":[0.0966193271554473,0.034333302044080236,0.6645079146282771],"reprojection_error_px":0.11586908974205057},{"quaternion_xyzw":[0.043054379791183726,-0.00039187997140417615,-0.44190387424627575,0.8960285334390955],"translation_xyz_m":[0.09664871575382844,0.034355449666508484,0.6644756428725503],"reprojection_error_px":0.32511743298701623}]}]},{"stamp_ns":1789379938544867491,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11670465661560794}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.151940671095458,0.106298598109822,-0.430528905229807,0.8833229886449081],"translation_xyz_m":[0.0966159823874595,0.034322414622354615,0.6644963132677421],"reprojection_error_px":0.11503886147585386},{"quaternion_xyzw":[0.04321235690430905,-0.000131905059759673,-0.4418969947849861,0.8960243974422949],"translation_xyz_m":[0.09664535859767384,0.034344672591736104,0.6644643772902463],"reprojection_error_px":0.3227999521403369}]}]},{"stamp_ns":1789379938578260029,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05086409321885855}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15213382931573036,0.1061885062316646,-0.430489164381741,0.8833223525259226],"translation_xyz_m":[0.0966138018077533,0.03431695047235428,0.6644701955451215],"reprojection_error_px":0.10692690094754806},{"quaternion_xyzw":[0.043414592619010144,-1.3884613500804872e-05,-0.4418925363288972,0.8960168298038588],"translation_xyz_m":[0.09664317584071853,0.034339294697142184,0.6644382926689912],"reprojection_error_px":0.3198138932616014}]}]},{"stamp_ns":1789379938611774792,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11025201377228533}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15217515858427808,0.10635698570836617,-0.42860628098632986,0.8842101382589703],"translation_xyz_m":[0.09642665573735694,0.03453888965101239,0.6644872499821544],"reprojection_error_px":0.12409952986647438},{"quaternion_xyzw":[0.0435098551115907,-0.00035214415335731945,-0.4399606540196283,0.8969623132647684],"translation_xyz_m":[0.09645602185329973,0.034561254299754186,0.6644549515155282],"reprojection_error_px":0.32820951531831893}]}]},{"stamp_ns":1789379938644895891,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06434358922526223}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1528156808362287,0.1052036889238816,-0.4266728764558165,0.8851721911720265],"translation_xyz_m":[0.09625016727509414,0.03480225205031644,0.664508546980927],"reprojection_error_px":0.10958648631149133},{"quaternion_xyzw":[0.0441421504060086,0.0006265099118005566,-0.43812776901414646,0.8978280102901394],"translation_xyz_m":[0.09627928073185012,0.0348250749059073,0.6644765611487602],"reprojection_error_px":0.32130177463301324}]}]},{"stamp_ns":1789379938678259268,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08748139588905682}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1526123717025266,0.10634123736063435,-0.424610758952044,0.8860624744464175],"translation_xyz_m":[0.09602132614359173,0.03505195953751725,0.66443422854414],"reprojection_error_px":0.11877697970223522},{"quaternion_xyzw":[0.04399876573053591,-0.0007124956108659295,-0.43592386269332906,0.8989071069352615],"translation_xyz_m":[0.09605047206995297,0.035074481029902814,0.6644001483124498],"reprojection_error_px":0.33689571838560317}]}]},{"stamp_ns":1789379938711726670,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05085421388062452}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1532847777449908,0.10570753124080667,-0.4224738331750227,0.8870431528584423],"translation_xyz_m":[0.09579665444736804,0.0353005084855806,0.6644280838757807],"reprojection_error_px":0.12118353038566233},{"quaternion_xyzw":[0.04472591917015296,-0.00028853731589127663,-0.4338621889428086,0.899868384768771],"translation_xyz_m":[0.09582590014806304,0.03532349001973434,0.6643955033256882],"reprojection_error_px":0.32869248999729256}]}]},{"stamp_ns":1789379938745130589,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.063076072319638}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15389455537978902,0.1043631870711499,-0.4198721952599204,0.8883310929244932],"translation_xyz_m":[0.09553852276234893,0.03562306045652654,0.6644126261702995],"reprojection_error_px":0.120015008037544},{"quaternion_xyzw":[0.045364603500283165,0.0008203642293640066,-0.43135178628467885,0.9010421833747907],"translation_xyz_m":[0.0955674843772868,0.03564655946755881,0.6643806411853387],"reprojection_error_px":0.3248945095055369}]}]},{"stamp_ns":1789379938778565280,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.048707710574198204}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15390108225725915,0.10486212580615907,-0.4175708186902068,0.8893553861251688],"translation_xyz_m":[0.09530548655548957,0.035902300207289635,0.6644123526271962],"reprojection_error_px":0.1268206479273276},{"quaternion_xyzw":[0.04542504139194912,0.00011232470524135921,-0.4289664960349555,0.9021775314632884],"translation_xyz_m":[0.09533427499614565,0.035925664546740545,0.6643781289609733],"reprojection_error_px":0.340681654970348}]}]},{"stamp_ns":1789379938811750104,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07156367308356995}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15469073316621057,0.10431430290590926,-0.41539634861699753,0.890300610376866],"translation_xyz_m":[0.09506938032804006,0.03617475891342694,0.6643789169633634],"reprojection_error_px":0.12825171929376591},{"quaternion_xyzw":[0.046241490786725875,0.00042693719036470155,-0.42687360471219565,0.9031281569381351],"translation_xyz_m":[0.09509820792540623,0.03619856248009902,0.6643454783382977],"reprojection_error_px":0.33648318722262077}]}]},{"stamp_ns":1789379938845087477,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06477392013441781}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15548332947923243,0.10345224132002837,-0.4131964486449071,0.8912862967908525],"translation_xyz_m":[0.09485904151864345,0.03642867215577749,0.664373469020113],"reprojection_error_px":0.12366726281025935},{"quaternion_xyzw":[0.0470763294763494,0.0011040853477862324,-0.4247801777041868,0.9040710153678074],"translation_xyz_m":[0.09488769418011897,0.03645293588592953,0.6643401231532952],"reprojection_error_px":0.3342506634160502}]}]},{"stamp_ns":1789379938878457759,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05026955820481525}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15494475755044634,0.1040254998338441,-0.41141182395396525,0.8921385142469883],"translation_xyz_m":[0.0946662258864495,0.036645708643814244,0.6643965836164718],"reprojection_error_px":0.10982956996140063},{"quaternion_xyzw":[0.04658347487862363,0.000365574253614627,-0.4228408352635226,0.905005786864009],"translation_xyz_m":[0.09469513320096981,0.03666981938167781,0.6643643440203796],"reprojection_error_px":0.3226039316543798}]}]},{"stamp_ns":1789379938911643187,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11745865774488154}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15607347239240996,0.10353321681924849,-0.4077263724190128,0.8936896270319281],"translation_xyz_m":[0.09427881599471832,0.03703945595680433,0.6642857329521493],"reprojection_error_px":0.12215922948048046},{"quaternion_xyzw":[0.047829698159246585,0.0005315011890604024,-0.419253970741745,0.9066080440288192],"translation_xyz_m":[0.09430755672176365,0.03706406642251778,0.664252484052472],"reprojection_error_px":0.3329030683032886}]}]},{"stamp_ns":1789379938945001761,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.18714180219466783}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1561672398020856,0.10263875097535437,-0.4039894842615808,0.8954717062067784],"translation_xyz_m":[0.09387654719774459,0.037554956295812215,0.6642690770769717],"reprojection_error_px":0.12383117363503388},{"quaternion_xyzw":[0.04790353565624552,0.0010281394327939656,-0.4154673398933833,0.9083452447626142],"translation_xyz_m":[0.09390494442666887,0.03757980669380769,0.6642355859922101],"reprojection_error_px":0.33524262724716425}]}]},{"stamp_ns":1789379938978315496,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09459338233271333}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15702689840146503,0.1022492235898924,-0.4017728825588992,0.8963627615502515],"translation_xyz_m":[0.09364258535764518,0.03775397718130698,0.6642046439865141],"reprojection_error_px":0.13877325467607657},{"quaternion_xyzw":[0.04887635960209453,0.0012359371756082216,-0.41334590319627335,0.909260544751724],"translation_xyz_m":[0.09367094641851688,0.03777923676730681,0.6641710531890985],"reprojection_error_px":0.3414366519403761}]}]},{"stamp_ns":1789379939011696998,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.21449809334213676}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15747031987126228,0.10147999546518395,-0.39884953878711543,0.8976769765841909],"translation_xyz_m":[0.09335709226322884,0.0380595755103624,0.664271142518772],"reprojection_error_px":0.1352740116727478},{"quaternion_xyzw":[0.049425058113877554,0.0017724119856571618,-0.41046121091674936,0.9105358952391436],"translation_xyz_m":[0.0933850955968875,0.038085132793762404,0.6642366081317214],"reprojection_error_px":0.345639121743854}]}]},{"stamp_ns":1789379939045189973,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.12397327089736203}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15782491198920923,0.10126664654625564,-0.39585640086345947,0.8989627763975003],"translation_xyz_m":[0.0930057435900999,0.03847359599783972,0.6642170080464583],"reprojection_error_px":0.130918573931208},{"quaternion_xyzw":[0.04976213770378165,0.0016264646932391657,-0.4074340580306835,0.9118764020526035],"translation_xyz_m":[0.09303387689052685,0.03849942971278914,0.6641836579324837],"reprojection_error_px":0.3369532892217687}]}]},{"stamp_ns":1789379939078685168,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11565559384202664}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15773581277073523,0.1011532345851752,-0.39415845062304633,0.8997369350565484],"translation_xyz_m":[0.09280894474738392,0.038656012899736164,0.6642023652043884],"reprojection_error_px":0.13960412154703078},{"quaternion_xyzw":[0.04973648195881104,0.0015726800407231492,-0.4056858499023627,0.9126570003176703],"translation_xyz_m":[0.09283686501284132,0.03868182333016098,0.6641679232670122],"reprojection_error_px":0.34683438216733986}]}]},{"stamp_ns":1789379939111836595,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09710297638878336}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15884066129571173,0.10086839561644331,-0.39090388250332614,0.9009935436665079],"translation_xyz_m":[0.09245462726242151,0.039038628369045904,0.6641139943551412],"reprojection_error_px":0.1282346822706061},{"quaternion_xyzw":[0.05088581225066767,0.0015336898036151468,-0.4025129710100525,0.9139975875656547],"translation_xyz_m":[0.09248268599150927,0.039064966925330576,0.664080308101314],"reprojection_error_px":0.33774243888643096}]}]},{"stamp_ns":1789379939145078221,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11572020753220016}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15841722614630394,0.1006427826481521,-0.3883068813787729,0.9022154834820998],"translation_xyz_m":[0.09220300728906694,0.039316185190492674,0.6642386675516762],"reprojection_error_px":0.12775468122048725},{"quaternion_xyzw":[0.05054857396669801,0.001558827744277185,-0.39980877531853215,0.915202357352893],"translation_xyz_m":[0.09223088064372821,0.03934248988557374,0.6642048592028796],"reprojection_error_px":0.3384470185858573}]}]},{"stamp_ns":1789379939178586574,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04761797642090749}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15928694987765807,0.1001660586486625,-0.38565094256182414,0.9032539946187018],"translation_xyz_m":[0.09189897825815742,0.03960534363190936,0.6641470121309263],"reprojection_error_px":0.14700828872987912},{"quaternion_xyzw":[0.0514857458244725,0.0017622901644059393,-0.3972294611102538,0.9162722671435231],"translation_xyz_m":[0.09192659132600771,0.03963198798800367,0.6641117328947239],"reprojection_error_px":0.3544164795544851}]}]},{"stamp_ns":1789379939211814771,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.1340524817398207}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15974645544490917,0.10033428065337614,-0.3816275343166458,0.9048616066283035],"translation_xyz_m":[0.0914366388324368,0.040078329335640636,0.6641690090050694],"reprojection_error_px":0.14470543469752048},{"quaternion_xyzw":[0.05202926534985503,0.0011592869255872807,-0.39314381257510567,0.9180030251774166],"translation_xyz_m":[0.09146423121374987,0.040105149316519643,0.6641329324342559],"reprojection_error_px":0.3577203471143797}]}]},{"stamp_ns":1789379939245069165,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0691260734416627}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.15939766849796205,0.09741701631801569,-0.37767723185629104,0.9068970265396611],"translation_xyz_m":[0.09103407398314949,0.04053212648531254,0.6644461737066771],"reprojection_error_px":0.1338269128891195},{"quaternion_xyzw":[0.051752153000561726,0.0037182851442579988,-0.38921518269511185,0.9196844190128498],"translation_xyz_m":[0.09106129964194944,0.040559676320756785,0.6644152703961907],"reprojection_error_px":0.32392243525548553}]}]},{"stamp_ns":1789379939278472481,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.076139748174249}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16074363633705646,0.09799685259099597,-0.37446014743127104,0.9079304479117052],"translation_xyz_m":[0.09063777729986863,0.04086593223198962,0.6640752299750935],"reprojection_error_px":0.12980636750878585},{"quaternion_xyzw":[0.05317588303211873,0.00281496025944633,-0.38606145238718054,0.9209348274678251],"translation_xyz_m":[0.09066493799332025,0.04089374114345799,0.6640412298485688],"reprojection_error_px":0.34048948258121275}]}]},{"stamp_ns":1789379939311776546,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.18173017924248674}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16277910740324494,0.09735254430310741,-0.3707228742035283,0.9091699482786425],"translation_xyz_m":[0.090240532887422,0.04118409834472612,0.6638826092503063],"reprojection_error_px":0.15769388756474728},{"quaternion_xyzw":[0.05537757656088101,0.003154751170328852,-0.3825572226050019,0.9222653322076266],"translation_xyz_m":[0.09026747239053515,0.04121267798680296,0.6638468934533274],"reprojection_error_px":0.36123308372760365}]}]},{"stamp_ns":1789379939345119682,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06481428125127485}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16249269502705055,0.0980791645027497,-0.3677746281696541,0.9103397302260081],"translation_xyz_m":[0.08988700100828131,0.041567168402203376,0.6639284337584617],"reprojection_error_px":0.1642491921962982},{"quaternion_xyzw":[0.055104040853307654,0.0020766756460014607,-0.3794295434544478,0.9235759057347954],"translation_xyz_m":[0.08991395916205806,0.04159553299923185,0.663891502583562],"reprojection_error_px":0.3705516507953363}]}]},{"stamp_ns":1789379939378459003,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.19282007500501402}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1626382467033735,0.09740168518737095,-0.3643483452311294,0.9117631248077818],"translation_xyz_m":[0.08946468736820871,0.041958608454751745,0.6639149261079952],"reprojection_error_px":0.13385705646341842},{"quaternion_xyzw":[0.05531771401033701,0.0023740334304322842,-0.37596254041478044,0.9249791795963876],"translation_xyz_m":[0.08949158814880516,0.04198728538881542,0.6638793106141284],"reprojection_error_px":0.3509716376851081}]}]},{"stamp_ns":1789379939411905705,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07592830134434891}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16349336446804708,0.09646582861218365,-0.359725940930069,0.9135433821702501],"translation_xyz_m":[0.08895612818988843,0.042395323666484866,0.663950681693061],"reprojection_error_px":0.1453972164766581},{"quaternion_xyzw":[0.056352735565554854,0.0028908508201819995,-0.3713973299283517,0.9267578084364356],"translation_xyz_m":[0.08898283437562639,0.0424245769759613,0.663915554086302],"reprojection_error_px":0.3527192689152796}]}]},{"stamp_ns":1789379939445288693,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05406692737112679}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1636763221080819,0.09556451293026394,-0.3558674393788924,0.9151152118938267],"translation_xyz_m":[0.08853984148175043,0.04281463869408286,0.6640047643809096],"reprojection_error_px":0.14763485043838068},{"quaternion_xyzw":[0.05660355946906989,0.0034358159665341453,-0.36751522987685953,0.9282870181321263],"translation_xyz_m":[0.08856635566094813,0.04284424114147512,0.6639706892099259],"reprojection_error_px":0.3478233468962215}]}]},{"stamp_ns":1789379939478519842,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04595702142886969}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1641794621314722,0.09513097438613588,-0.3533312876736321,0.9160525110917266],"translation_xyz_m":[0.08824541676271827,0.04306668715448574,0.663972261559844],"reprojection_error_px":0.16784075836214207},{"quaternion_xyzw":[0.05718840742864589,0.003615792870951144,-0.36500499615154947,0.929240423616029],"translation_xyz_m":[0.08827141031916419,0.04309638312634393,0.6639350963971932],"reprojection_error_px":0.37372463449418053}]}]},{"stamp_ns":1789379939511833231,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11018730840975571}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16449477032905302,0.09525894864293315,-0.3508977243946537,0.9169176572912581],"translation_xyz_m":[0.0879552813329681,0.04332579486300305,0.6639280205067236],"reprojection_error_px":0.15812027156210673},{"quaternion_xyzw":[0.05755575164628425,0.0032362027076676186,-0.36253528808250624,0.9301854800734101],"translation_xyz_m":[0.08798145646858627,0.04335569730636543,0.663891781615517],"reprojection_error_px":0.36430394470560384}]}]},{"stamp_ns":1789379939545482067,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.1226302131161459}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16552782944354663,0.09438542305006957,-0.3465230662400981,0.9184844550447918],"translation_xyz_m":[0.08744696306101232,0.043720660900295326,0.6638614241358548],"reprojection_error_px":0.155529901118774},{"quaternion_xyzw":[0.058762165219473285,0.0036988110576812103,-0.3582408215520607,0.9317708090025054],"translation_xyz_m":[0.08747307313709195,0.043751224373002905,0.6638263465434824],"reprojection_error_px":0.3567060186807607}]}]},{"stamp_ns":1789379939578601064,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09852519204421335}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16574371627106002,0.09403137612267365,-0.3421284021262003,0.9201278592020283],"translation_xyz_m":[0.08691121601709192,0.04418429690659499,0.6638630850665848],"reprojection_error_px":0.16075725288723533},{"quaternion_xyzw":[0.05908589439510823,0.003577623253648108,-0.3537676907813598,0.9334583432883383],"translation_xyz_m":[0.08693695528116327,0.04421496992088455,0.6638262371498782],"reprojection_error_px":0.3687642025061605}]}]},{"stamp_ns":1789379939611985431,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0992703296013795}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16647338088312796,0.09331485364325318,-0.3375945020653442,0.9217423195894456],"translation_xyz_m":[0.0863701971883777,0.04464379622386921,0.6637548552770268],"reprojection_error_px":0.15225094836469363},{"quaternion_xyzw":[0.059915291788388926,0.003837870507648467,-0.34924753952431814,0.9351051195967695],"translation_xyz_m":[0.08639581116150108,0.04467496125972148,0.6637186330684407],"reprojection_error_px":0.3617504507278593}]}]},{"stamp_ns":1789379939645357624,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.13055391670347954}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1670652557929083,0.09261045375076898,-0.3329882306684929,0.9233803887884957],"translation_xyz_m":[0.08585393699013665,0.0450650251895414,0.6638745890997229],"reprojection_error_px":0.15106071423386697},{"quaternion_xyzw":[0.06068545117154337,0.004116164430224264,-0.3446445686702713,0.9367606174965792],"translation_xyz_m":[0.08587923216382473,0.04509655679941371,0.6638377263354361],"reprojection_error_px":0.3647419781810007}]}]},{"stamp_ns":1789379939678522484,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.050267775415629656}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16827625395992066,0.09248578924196829,-0.32827317983498794,0.9248600978215744],"translation_xyz_m":[0.08525705839172613,0.04552767470722005,0.6636120898241991],"reprojection_error_px":0.15159852607076585},{"quaternion_xyzw":[0.06200120779172092,0.0037335793055003124,-0.33995440956999634,0.9383884643535706],"translation_xyz_m":[0.08528251803407064,0.045559800800738556,0.6635760901635176],"reprojection_error_px":0.35995830838400067}]}]},{"stamp_ns":1789379939711896795,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08101264332463563}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16863621218287234,0.09115325610046081,-0.32372443566858117,0.9265286836325984],"translation_xyz_m":[0.08470833012151593,0.04596089516573521,0.6636841832745678],"reprojection_error_px":0.16102578507068058},{"quaternion_xyzw":[0.06250466717545511,0.004593429665273221,-0.33541126582907743,0.9399847603765349],"translation_xyz_m":[0.08473324665767268,0.045993386581230936,0.6636476296676589],"reprojection_error_px":0.3672822498166016}]}]},{"stamp_ns":1789379939745297162,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08627403644983657}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16889737054673037,0.09055418114432812,-0.3195627541453402,0.9279834398644867],"translation_xyz_m":[0.08420854120134348,0.04634553284054423,0.6637272802858686],"reprojection_error_px":0.15133298085447028},{"quaternion_xyzw":[0.06291089930556101,0.004778471261024749,-0.33120748710805864,0.9414462201552133],"translation_xyz_m":[0.08423297772216679,0.04637815681178169,0.6636887675215424],"reprojection_error_px":0.3738916767646916}]}]},{"stamp_ns":1789379939778962270,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0899817687023042}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.16952338843391432,0.08975203461924505,-0.31488599024002484,0.9295446230311641],"translation_xyz_m":[0.08364374579969336,0.04679428288409403,0.663718241831408],"reprojection_error_px":0.16247482635295885},{"quaternion_xyzw":[0.06365484114756527,0.005096374099960219,-0.32652898443238226,0.9430275237208448],"translation_xyz_m":[0.08366786538110033,0.0468272946508492,0.6636793202718426],"reprojection_error_px":0.3806839635260335}]}]},{"stamp_ns":1789379939812098364,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.12318742329182622}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17016289555066452,0.08957380525784574,-0.31010278322634227,0.9310517634507328],"translation_xyz_m":[0.08307000532013842,0.04718277965363076,0.6636782235841094],"reprojection_error_px":0.1562318210957686},{"quaternion_xyzw":[0.06449925801823182,0.004825041436650406,-0.3217110934885432,0.9446261361071046],"translation_xyz_m":[0.08309428224842717,0.047216278626579226,0.6636410107583846],"reprojection_error_px":0.3687850894742306}]}]},{"stamp_ns":1789379939845399749,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04540783838056302}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17125970905954752,0.08845923870935421,-0.30619828986137343,0.9322487234776838],"translation_xyz_m":[0.08256124905434165,0.047552815211482614,0.6635473001600948],"reprojection_error_px":0.16132872798261766},{"quaternion_xyzw":[0.06568453679837288,0.005492108430771007,-0.31790070917794594,0.9458300679692556],"translation_xyz_m":[0.08258520479435108,0.04758689497797638,0.6635099773382642],"reprojection_error_px":0.37159551422352416}]}]},{"stamp_ns":1789379939878793511,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05976846417347205}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1711793524498607,0.08820861679596995,-0.3018170780097706,0.9337147961981925],"translation_xyz_m":[0.08204382427498184,0.04792871358684942,0.6636886922483641],"reprojection_error_px":0.1611159966059511},{"quaternion_xyzw":[0.0657759729828563,0.00532220576966454,-0.3134056713658447,0.9473236409272324],"translation_xyz_m":[0.08206762041244658,0.04796291032493545,0.6636514084924748],"reprojection_error_px":0.37130482667357206}]}]},{"stamp_ns":1789379939912272464,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07475095586281028}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17222580543359095,0.08700043890219565,-0.2973964355105563,0.9350532368369149],"translation_xyz_m":[0.0814868992058949,0.04830404473123544,0.6636288266460929],"reprojection_error_px":0.16235439681834518},{"quaternion_xyzw":[0.06697806767465748,0.00606368263292933,-0.3090721216943471,0.948657785397269],"translation_xyz_m":[0.08151015949395966,0.04833874052757855,0.6635901600250055],"reprojection_error_px":0.37926711361820253}]}]},{"stamp_ns":1789379939945656318,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07071203375400224}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17253184761837276,0.0860031688595875,-0.2931935775520029,0.9364153686189466],"translation_xyz_m":[0.08097668565365455,0.0486872166869613,0.6637109454473625],"reprojection_error_px":0.15245221005762977},{"quaternion_xyzw":[0.06739628978330095,0.006630347455715355,-0.3048547233271279,0.9499880927049571],"translation_xyz_m":[0.08099973716388967,0.048722324748715046,0.6636736678600038],"reprojection_error_px":0.3678228854657441}]}]},{"stamp_ns":1789379939978978870,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06670559632516349}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17255183254215387,0.08608513472415752,-0.28986750448771703,0.9374390884254461],"translation_xyz_m":[0.08056107292172944,0.04895051094118682,0.6636954897399989],"reprojection_error_px":0.16048615011789163},{"quaternion_xyzw":[0.06755605504524148,0.00621970024494292,-0.30143128191481766,0.9510713312043303],"translation_xyz_m":[0.08058403084668325,0.04898565916790516,0.6636575906968794],"reprojection_error_px":0.37450666001198496}]}]},{"stamp_ns":1789379940012279362,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07778497670380392}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17310527062738834,0.08478352457424941,-0.2857418078332716,0.9387214381795912],"translation_xyz_m":[0.08004609682369523,0.04931491603024498,0.6636817919127295],"reprojection_error_px":0.16475357879353533},{"quaternion_xyzw":[0.06821671531956353,0.0070883475377637815,-0.29734452919123067,0.95230376773393],"translation_xyz_m":[0.08006859509492951,0.04935048677304002,0.6636438659376657],"reprojection_error_px":0.37668868361725516}]}]},{"stamp_ns":1789379940045480128,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05449712651747182}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17367981671412253,0.08439589568809866,-0.28141507868502114,0.939956492368586],"translation_xyz_m":[0.07947788858036041,0.049657175394961846,0.6636475565014293],"reprojection_error_px":0.16658889379401082},{"quaternion_xyzw":[0.06896975322199927,0.0070022322633953235,-0.2929862860287698,0.953600114347148],"translation_xyz_m":[0.07950017863381098,0.04969304230042277,0.6636090154272793],"reprojection_error_px":0.38068019988532237}]}]},{"stamp_ns":1789379940078914797,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05612165943757733}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17377067427299506,0.08434747252063902,-0.27696765310508686,0.9412641370920009],"translation_xyz_m":[0.07891727129726019,0.05002051451062432,0.6637240462051182],"reprojection_error_px":0.17133550935654931},{"quaternion_xyzw":[0.06922690895478753,0.006585827236973352,-0.2884214178962799,0.9549750508023075],"translation_xyz_m":[0.07893949282876914,0.050056525966614286,0.6636855644121333],"reprojection_error_px":0.3823830014825358}]}]},{"stamp_ns":1789379940112162019,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.10599173470969704}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17486088430770427,0.08355076811112766,-0.27267407462482857,0.9423862208853977],"translation_xyz_m":[0.07835866124089588,0.05035925686581956,0.6635814681484931],"reprojection_error_px":0.1488154810584214},{"quaternion_xyzw":[0.0704646070979354,0.006930802277036387,-0.28418986695750953,0.9561500000758266],"translation_xyz_m":[0.07838054527162897,0.05039575619144983,0.663542108680558],"reprojection_error_px":0.37756109839502006}]}]},{"stamp_ns":1789379940145446456,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0817730522351118}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17494696241396204,0.08304633208313437,-0.2684224593553211,0.9436345957961199],"translation_xyz_m":[0.07780625629725048,0.050683544879239616,0.6636089414292595],"reprojection_error_px":0.16364712315812424},{"quaternion_xyzw":[0.07072505270256023,0.006980269803694688,-0.2798585529839062,0.9574071407063152],"translation_xyz_m":[0.07782790799687808,0.05072022908913081,0.6635695573650516],"reprojection_error_px":0.3838214248860869}]}]},{"stamp_ns":1789379940178828922,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.10210740932260007}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1752483724470631,0.08234205637723618,-0.26423322517287984,0.9448219919227968],"translation_xyz_m":[0.07728591084456746,0.05100767506701847,0.6636434360894607],"reprojection_error_px":0.15803612551504867},{"quaternion_xyzw":[0.07117672751850711,0.007268082485752257,-0.275636444988084,0.9585956387502853],"translation_xyz_m":[0.077307348674999,0.05104468091576126,0.6636046902348446],"reprojection_error_px":0.37820123793891103}]}]},{"stamp_ns":1789379940212386113,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0679246947011374}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17613813481764848,0.08163922709670782,-0.2598724566520888,0.9459263715192744],"translation_xyz_m":[0.07672448539660377,0.05133982568226916,0.6636515482681397],"reprojection_error_px":0.15327046500587116},{"quaternion_xyzw":[0.07223289742495022,0.007503263881891428,-0.2713017006423831,0.9597507472199571],"translation_xyz_m":[0.07674559656271654,0.05137723710269829,0.6636119906590708],"reprojection_error_px":0.38044901560841177}]}]},{"stamp_ns":1789379940245602301,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09229477143162802}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17672632041473343,0.08156942072946745,-0.2554367525597809,0.9470302543827422],"translation_xyz_m":[0.07613602858782663,0.051670675702347985,0.6635718220434278],"reprojection_error_px":0.15286107340034769},{"quaternion_xyzw":[0.0729886954731784,0.0070922571447141975,-0.2668032721128769,0.9609570043511724],"translation_xyz_m":[0.07615701481871916,0.051708336503737365,0.6635315001502003],"reprojection_error_px":0.3841627059587546}]}]},{"stamp_ns":1789379940278762227,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09956765604585627}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1769871701900936,0.08023280463769311,-0.2507907592797407,0.9483365614105385],"translation_xyz_m":[0.07552660559228995,0.052009359415527374,0.663560669954357],"reprojection_error_px":0.16373269494615494},{"quaternion_xyzw":[0.07342287140513193,0.007931600808819708,-0.2621484891508438,0.9621976622810746],"translation_xyz_m":[0.07554721443905485,0.052047446982370696,0.6635215747007833],"reprojection_error_px":0.3825131925749237}]}]},{"stamp_ns":1789379940312223528,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06949980092952754}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1780978756843421,0.07868984215702608,-0.24616813296053786,0.9494684332471248],"translation_xyz_m":[0.07493955164849758,0.052322515411001407,0.6635577366282259],"reprojection_error_px":0.16215540233669298},{"quaternion_xyzw":[0.07473160877049426,0.009000556231859873,-0.25763749153595533,0.9633052992655815],"translation_xyz_m":[0.07495968441210993,0.05236126768667396,0.6635190822584387],"reprojection_error_px":0.3796211192907623}]}]},{"stamp_ns":1789379940345603681,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08522353097594813}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17838307330410724,0.07807811681458918,-0.2415789478403412,0.9506434130596091],"translation_xyz_m":[0.07434577044568894,0.05264096903216373,0.6635008703052273],"reprojection_error_px":0.1661953523147969},{"quaternion_xyzw":[0.07519277254002382,0.009145574404718276,-0.25299169659546017,0.9644986297970064],"translation_xyz_m":[0.0743656703579064,0.05268001160559194,0.6634625551171417],"reprojection_error_px":0.37968383356488916}]}]},{"stamp_ns":1789379940378851369,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06516158758352104}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17813037441462992,0.077642201715931,-0.2371800161598807,0.951833440344478],"translation_xyz_m":[0.07375235715934589,0.05295122025558126,0.6635607709239691],"reprojection_error_px":0.17470816696576533},{"quaternion_xyzw":[0.07512079861469226,0.009092621448585357,-0.24846016140063795,0.9656820066913603],"translation_xyz_m":[0.07377196414261025,0.0529902860537026,0.6635220289016398],"reprojection_error_px":0.38583459314308305}]}]},{"stamp_ns":1789379940412248155,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11334767478585141}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.17913766717378674,0.0767551521495133,-0.23250262429691299,0.9528698087950354],"translation_xyz_m":[0.07313274290167862,0.0532717646589832,0.6634642328878104],"reprojection_error_px":0.16365448434202973},{"quaternion_xyzw":[0.07630143256819569,0.009478705196246764,-0.24382184270373913,0.9667673735477407],"translation_xyz_m":[0.07315203576533091,0.0533113507931654,0.663425447508891],"reprojection_error_px":0.38120421161533374}]}]},{"stamp_ns":1789379940445455869,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07031230656401209}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1792120161727451,0.0763237359132074,-0.22758356685230557,0.9540772823489369],"translation_xyz_m":[0.07246874850246253,0.053589165744456754,0.6634963406191241],"reprojection_error_px":0.1514476570072005},{"quaternion_xyzw":[0.0765998388160485,0.009375256760666887,-0.23879327834540118,0.9679991422884023],"translation_xyz_m":[0.07248787422224433,0.053628977047894405,0.6634581260535719],"reprojection_error_px":0.37319810536205533}]}]},{"stamp_ns":1789379940479175331,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08427874572201159}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18025480230592908,0.07477544280294475,-0.22265239007394164,0.955166348126668],"translation_xyz_m":[0.0718325809202,0.053891368541557315,0.6634923503308985],"reprojection_error_px":0.16104255462364048},{"quaternion_xyzw":[0.07786648599849932,0.01041298612088253,-0.23395393916127166,0.9690685911890942],"translation_xyz_m":[0.07185100437891533,0.05393166983964162,0.6634527982255674],"reprojection_error_px":0.38428791974603355}]}]},{"stamp_ns":1789379940512433948,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06834663092854401}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18077705041348716,0.0736900382287292,-0.21763027377630337,0.9563087891709974],"translation_xyz_m":[0.07117869987237711,0.05421294697363031,0.6634851882665167],"reprojection_error_px":0.1542121627947235},{"quaternion_xyzw":[0.07858646997371452,0.010980003937991758,-0.22892388270746397,0.9702048557787842],"translation_xyz_m":[0.07119680328860356,0.05425371701888206,0.6634467464463266],"reprojection_error_px":0.37577515866817607}]}]},{"stamp_ns":1789379940545523778,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06208139305629151}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.181292916392502,0.07276135293464316,-0.21315869435552467,0.957288898403021],"translation_xyz_m":[0.07057569043262149,0.054489544813747015,0.6635016327275182],"reprojection_error_px":0.1352777445954716},{"quaternion_xyzw":[0.07929694082559564,0.011420013085915975,-0.22444357658193545,0.9711882718648988],"translation_xyz_m":[0.0705935299852883,0.05453074638209997,0.6634642072893999],"reprojection_error_px":0.36300038053206374}]}]},{"stamp_ns":1789379940579000830,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06149940560320997}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18121257146591563,0.07202835238787975,-0.2082998595204138,0.9584284474694883],"translation_xyz_m":[0.06993783221194277,0.05478998624221052,0.6635449816459903],"reprojection_error_px":0.14716610203105207},{"quaternion_xyzw":[0.07941849776425493,0.011647885234453714,-0.21948712259691314,0.9723077866584026],"translation_xyz_m":[0.06995518970027584,0.054831233170024245,0.6635064140697075],"reprojection_error_px":0.37391168589481055}]}]},{"stamp_ns":1789379940612432703,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.053007106810991314}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18188628638741008,0.07183797368411492,-0.20376347130191305,0.9592899103629475],"translation_xyz_m":[0.06929990499915037,0.05503298340110376,0.6633259276822255],"reprojection_error_px":0.14944688318434488},{"quaternion_xyzw":[0.08030845780284483,0.011337311017821785,-0.2148952740971393,0.973263601577152],"translation_xyz_m":[0.06931708782489458,0.0550745208974311,0.6632866031293516],"reprojection_error_px":0.37880514839555135}]}]},{"stamp_ns":1789379940645650328,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.052137860920788356}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18235696538873164,0.0713019003147836,-0.19901558253139276,0.960236832294723],"translation_xyz_m":[0.06864289572454806,0.05531628316613401,0.6633303205144748],"reprojection_error_px":0.1381631196257537},{"quaternion_xyzw":[0.08098198309545346,0.01133305540302929,-0.21008690374328842,0.9742571391294589],"translation_xyz_m":[0.06865995615484893,0.055358216174101284,0.6632921558105456],"reprojection_error_px":0.3682858331212772}]}]},{"stamp_ns":1789379940679060284,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05926951472963346}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18291066044101428,0.07028285313376326,-0.19412107678052803,0.961208103587344],"translation_xyz_m":[0.06798888095171146,0.05560015223982956,0.6633516560899605],"reprojection_error_px":0.14269685352056646},{"quaternion_xyzw":[0.08174514198042537,0.011823077817907743,-0.20518042042465295,0.9752327628150493],"translation_xyz_m":[0.06800539435822629,0.055642348039002906,0.6633122630079945],"reprojection_error_px":0.3766545713995791}]}]},{"stamp_ns":1789379940712550883,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05905730500744565}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18336176746186347,0.06943201946312463,-0.18909642084563558,0.9621851175994691],"translation_xyz_m":[0.06731380994562143,0.05585439144764848,0.6632451172653759],"reprojection_error_px":0.150544561314622},{"quaternion_xyzw":[0.082434882368615,0.012151986141204201,-0.20011937404890576,0.9762218270106291],"translation_xyz_m":[0.06733000575523261,0.0558969647086129,0.6632062585991665],"reprojection_error_px":0.3770159032790276}]}]},{"stamp_ns":1789379940745995096,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.042834939150324394}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18448356345814207,0.06871857584479533,-0.18427028435181142,0.9629579608957763],"translation_xyz_m":[0.06664600913405644,0.05611244117982852,0.6632692997054368],"reprojection_error_px":0.15006964912935505},{"quaternion_xyzw":[0.08378769620417996,0.012305236097041403,-0.1953090807158346,0.977078587483795],"translation_xyz_m":[0.06666202132480296,0.056155612186672796,0.6632313256185699],"reprojection_error_px":0.3719388506382711}]}]},{"stamp_ns":1789379940779011850,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06553569838818817}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18442838038606263,0.06795414366879779,-0.17930741961546542,0.9639591568823118],"translation_xyz_m":[0.06596752349091084,0.056366760671784054,0.6633325166691832],"reprojection_error_px":0.13405331021486377},{"quaternion_xyzw":[0.08397591664165244,0.012531346750910473,-0.1902473332257716,0.9780577503262703],"translation_xyz_m":[0.06598312782858715,0.056410049070888556,0.6632942406075168],"reprojection_error_px":0.3676519873370179}]}]},{"stamp_ns":1789379940812221775,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0653471161366563}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18483676645755226,0.06781477107812375,-0.17468568575291007,0.9647390516518823],"translation_xyz_m":[0.06529224650724778,0.0566079711945914,0.6632346262531298],"reprojection_error_px":0.12800115365989462},{"quaternion_xyzw":[0.08459345216071237,0.012114187771733193,-0.18552032415674766,0.978917465178214],"translation_xyz_m":[0.06530758504651754,0.056651366084365704,0.6631947277131067],"reprojection_error_px":0.3742391144636664}]}]},{"stamp_ns":1789379940845580793,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.049109640306819566}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1846709226168701,0.06713638540051463,-0.1697546086621124,0.9656980526713103],"translation_xyz_m":[0.0646256507664598,0.0568600087243311,0.6633045400178625],"reprojection_error_px":0.14728143343586325},{"quaternion_xyzw":[0.08464631070236213,0.012257413335107895,-0.18047177312922769,0.9798544264360926],"translation_xyz_m":[0.06464080684422219,0.05690364448024394,0.6632663990903166],"reprojection_error_px":0.3720812989653558}]}]},{"stamp_ns":1789379940879200538,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11170728018248603}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18557682955394997,0.06543313817226314,-0.16441353973151132,0.9665650173240228],"translation_xyz_m":[0.06391644407196878,0.057090103733988366,0.6632707998516943],"reprojection_error_px":0.14365299857325062},{"quaternion_xyzw":[0.08583159249340476,0.013409150746195102,-0.17520162523887065,0.9806923691555727],"translation_xyz_m":[0.06393085365911048,0.057134207089344184,0.6632319024950227],"reprojection_error_px":0.37491828411031614}]}]},{"stamp_ns":1789379940912546151,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.058229871300115796}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18608376907274146,0.06476315593740005,-0.15952521750221732,0.967331519956618],"translation_xyz_m":[0.06323065427268809,0.05732781247002735,0.6632574930556814],"reprojection_error_px":0.1527221944820098},{"quaternion_xyzw":[0.08656162417258843,0.013515966250072918,-0.17025671278272644,0.9814973538575388],"translation_xyz_m":[0.06324461730275556,0.05737209851427334,0.6632171498884699],"reprojection_error_px":0.38603344604496914}]}]},{"stamp_ns":1789379940945684704,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.1056554966915387}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18669510310599885,0.06414620493973613,-0.15452635094972578,0.968066015171606],"translation_xyz_m":[0.06252198854102077,0.05756362633609592,0.6632336468662551],"reprojection_error_px":0.1633443773045989},{"quaternion_xyzw":[0.08739986372635022,0.013545600180579116,-0.1651999067976362,0.9822865016533697],"translation_xyz_m":[0.06253575749265937,0.05760833416397823,0.6631942688075722],"reprojection_error_px":0.38535362080221564}]}]},{"stamp_ns":1789379940979063643,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.11758077310817557}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18751520351941142,0.06345232979464642,-0.14934367668053178,0.9687663890372363],"translation_xyz_m":[0.061810496618947164,0.05776840310198616,0.6631147679889602],"reprojection_error_px":0.15873100614896363},{"quaternion_xyzw":[0.08849133117042242,0.013685546766798857,-0.15999043857093356,0.9830488541691408],"translation_xyz_m":[0.06182379327209396,0.05781339510329505,0.6630737363486479],"reprojection_error_px":0.3919594476695426}]}]},{"stamp_ns":1789379941012627531,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.10328103225648413}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18811227193564398,0.061969072534803986,-0.14434969728627153,0.9695033636299301],"translation_xyz_m":[0.06109843183352312,0.05795422522111826,0.6630940018583722],"reprojection_error_px":0.14159615422308447},{"quaternion_xyzw":[0.08936731159030863,0.014591396967552144,-0.15501478280330086,0.9837535219078515],"translation_xyz_m":[0.061111282024145175,0.0579997557443817,0.6630546585330012],"reprojection_error_px":0.37676330935860164}]}]},{"stamp_ns":1789379941045925396,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07224691591712004}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18827297487951855,0.06140948942164215,-0.13931888118132582,0.970243480207609],"translation_xyz_m":[0.06041175372456634,0.0581635724994671,0.6631180615337987],"reprojection_error_px":0.14762440506393118},{"quaternion_xyzw":[0.08977241017495641,0.014606844132717225,-0.14988397949868304,0.9845112224680327],"translation_xyz_m":[0.0604242180399971,0.05820920872246579,0.6630778554367699],"reprojection_error_px":0.38365120898115684}]}]},{"stamp_ns":1789379941079158562,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06691662241891723}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18869199602743864,0.060953142155599375,-0.13429288627807176,0.9708993077511412],"translation_xyz_m":[0.059678935796631354,0.0583683232104286,0.663036074690286],"reprojection_error_px":0.14177352360767784},{"quaternion_xyzw":[0.0904317525168522,0.014463228053413196,-0.14476179764226113,0.9852192320059463],"translation_xyz_m":[0.05969104317663622,0.058414112362731606,0.662994621786857],"reprojection_error_px":0.38796584721311145}]}]},{"stamp_ns":1789379941112470258,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05506939572059775}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.18954828907762866,0.05981026231606951,-0.12918460586594493,0.9714965343410584],"translation_xyz_m":[0.058969406841154835,0.058545523225895464,0.6631001849555115],"reprojection_error_px":0.15234353882302334},{"quaternion_xyzw":[0.09158047686038422,0.015021669077333847,-0.1396610681802717,0.985840834897247],"translation_xyz_m":[0.058981010642536615,0.05859173239156193,0.6630584945744006],"reprojection_error_px":0.39312321096848113}]}]},{"stamp_ns":1789379941145751882,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07364107625292989}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1894261925077359,0.05905623429372072,-0.12395140991214604,0.9722479759628501],"translation_xyz_m":[0.058226067954027745,0.05873961481688507,0.6631468429433955],"reprojection_error_px":0.151762837432932},{"quaternion_xyzw":[0.09172341268615684,0.015172981361062956,-0.13430487553008766,0.9865692051805942],"translation_xyz_m":[0.05823733791109756,0.05878597484183568,0.6631057809749653],"reprojection_error_px":0.3898924565150547}]}]},{"stamp_ns":1789379941179109704,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.053557473122276}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1904595033522881,0.05806567799700587,-0.11872943896717053,0.9727573566640804],"translation_xyz_m":[0.05749838152614621,0.05889433281177251,0.6629642935123349],"reprojection_error_px":0.1556389539433241},{"quaternion_xyzw":[0.09304491594935325,0.015593283553382322,-0.12909134385196006,0.9871347010750339],"translation_xyz_m":[0.05750909921411825,0.05894107046606542,0.6629218307415157],"reprojection_error_px":0.39855193597377025}]}]},{"stamp_ns":1789379941212772456,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.051659070217209245}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19050034798706367,0.05723377913667312,-0.11355740486097719,0.9734159582335715],"translation_xyz_m":[0.0567656781084984,0.05908360835815494,0.6630732093896037],"reprojection_error_px":0.1611353014010511},{"quaternion_xyzw":[0.09334448767231325,0.01581959090302753,-0.12381719018273166,0.9877782395761244],"translation_xyz_m":[0.05677594164110149,0.05913043967142238,0.6630302432296975],"reprojection_error_px":0.4033236419637252}]}]},{"stamp_ns":1789379941246038516,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07689764500631194}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19086562354839412,0.05657533948067711,-0.10853107197307323,0.9739561340874159],"translation_xyz_m":[0.05605843714853088,0.05924440891070447,0.6630028453411848],"reprojection_error_px":0.1561756008090418},{"quaternion_xyzw":[0.09396184870653418,0.015916084863316696,-0.11871158425142327,0.9883447824493188],"translation_xyz_m":[0.05606841658815368,0.059291540255135736,0.6629604989009267],"reprojection_error_px":0.3983946692789674}]}]},{"stamp_ns":1789379941279059654,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04688660261823216}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19165693401813674,0.055840957052207645,-0.10370590699932954,0.9743687659257697],"translation_xyz_m":[0.0553671940871078,0.05937291679643502,0.6630066689698192],"reprojection_error_px":0.15278795535359574},{"quaternion_xyzw":[0.09504257841429578,0.016080961458323505,-0.11385295741917897,0.9888103028659466],"translation_xyz_m":[0.05537681711692623,0.05942041530388036,0.6629641416718765],"reprojection_error_px":0.39794926040388545}]}]},{"stamp_ns":1789379941312522603,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04962194477408474}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19212539486210078,0.05412778242555694,-0.09909590743432051,0.9748528180953995],"translation_xyz_m":[0.05472115376174514,0.05950281090693261,0.6629750134585756],"reprojection_error_px":0.15613027266299684},{"quaternion_xyzw":[0.09575447302691872,0.01727274189490232,-0.10927895573446639,0.9892375059187525],"translation_xyz_m":[0.05473006518209946,0.05955061376625187,0.6629324302213466],"reprojection_error_px":0.39980042008903494}]}]},{"stamp_ns":1789379941345732568,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.056688875133875896}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19191853738588577,0.054241515104727814,-0.0941789338777347,0.9753745236884441],"translation_xyz_m":[0.054032323722837526,0.0596378828113456,0.6630617726444265],"reprojection_error_px":0.15697395940625306},{"quaternion_xyzw":[0.09582503269454953,0.016607547583074003,-0.10416785358303628,0.989793317189133],"translation_xyz_m":[0.054041113317398436,0.05968565452693434,0.6630187181600096],"reprojection_error_px":0.40245921853090333}]}]},{"stamp_ns":1789379941379220688,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.056964139295242244}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1924486231061052,0.05376603774716145,-0.0892890665870894,0.9757562212138549],"translation_xyz_m":[0.05333183303772042,0.05976880871876839,0.6631533875594894],"reprojection_error_px":0.15938495805518238},{"quaternion_xyzw":[0.09663534891886677,0.01649717301730266,-0.09919182892320295,0.9902274656343542],"translation_xyz_m":[0.053340312282573854,0.05981680068553496,0.6631097609882252],"reprojection_error_px":0.4061197188042994}]}]},{"stamp_ns":1789379941412437427,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04803329437642238}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19332296631018164,0.05200833586537512,-0.08440395868576744,0.9761133824795755],"translation_xyz_m":[0.05262831365108059,0.05987980817149355,0.6629927395289232],"reprojection_error_px":0.17535984412924444},{"quaternion_xyzw":[0.09777335260547018,0.01767968700887169,-0.09436475460365452,0.9905670564258366],"translation_xyz_m":[0.052635969215907775,0.05992812139987453,0.6629477540403563],"reprojection_error_px":0.4193797273103531}]}]},{"stamp_ns":1789379941445775520,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0746463096555263}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19357800060229996,0.05143894288473091,-0.07944643923142151,0.9765090148744995],"translation_xyz_m":[0.051922460812131005,0.059983567304472935,0.6630493436622746],"reprojection_error_px":0.16404186007881016},{"quaternion_xyzw":[0.09833043868659914,0.017675448441749397,-0.089307404394874,0.990980772200114],"translation_xyz_m":[0.05192974669018953,0.06003200395408863,0.663003646207327],"reprojection_error_px":0.41823321673048686}]}]},{"stamp_ns":1789379941479141744,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05045453496114918}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19410169146551265,0.05112317580060574,-0.07480083017188863,0.9767885083639567],"translation_xyz_m":[0.05121931635622546,0.060084565008956495,0.6629148680651257],"reprojection_error_px":0.1679550986165528},{"quaternion_xyzw":[0.09910303703871821,0.01740115327254938,-0.08455873592825293,0.9913251777760432],"translation_xyz_m":[0.05122646946040123,0.06013334099006791,0.662869981725198],"reprojection_error_px":0.4159349340420502}]}]},{"stamp_ns":1789379941512580990,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0633480901693714}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19438785603497688,0.04992168869741717,-0.06960552293864673,0.977177700114038],"translation_xyz_m":[0.050489662517915486,0.06018129200443895,0.662883453135987],"reprojection_error_px":0.16543079393370583},{"quaternion_xyzw":[0.09968840035704296,0.018018979427391932,-0.07931647758355109,0.9916886787688902],"translation_xyz_m":[0.05049624206286859,0.060230264804160716,0.6628382954487164],"reprojection_error_px":0.41644388257842774}]}]},{"stamp_ns":1789379941545799953,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09617448576702362}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19430218002799476,0.04949634727811811,-0.0648023031406652,0.9775466413170054],"translation_xyz_m":[0.0498073209101914,0.06028541258337396,0.6629921049330832],"reprojection_error_px":0.15936235577140487},{"quaternion_xyzw":[0.09987058948562623,0.0178856536609329,-0.07437592388699449,0.9920555381100751],"translation_xyz_m":[0.04981356600746441,0.06033436370273053,0.662946105387957],"reprojection_error_px":0.4181606303853942}]}]},{"stamp_ns":1789379941579151198,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05621364079037423}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1952127910315757,0.048807332961603576,-0.05960920545892027,0.9777303069310033],"translation_xyz_m":[0.04905026275063786,0.0603681961885259,0.6629583716257286],"reprojection_error_px":0.1591712329854661},{"quaternion_xyzw":[0.1010929828229972,0.017940342997769525,-0.06912652265968966,0.9923103732109481],"translation_xyz_m":[0.049056210566028706,0.06041761061711712,0.6629130349757257],"reprojection_error_px":0.41485409564742143}]}]},{"stamp_ns":1789379941612661683,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.060332823285570276}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1952903012591633,0.0479023650894855,-0.054514915372569236,0.9780568417299991],"translation_xyz_m":[0.04832932184641567,0.06044198593093219,0.6630221686906189],"reprojection_error_px":0.17104569518825163},{"quaternion_xyzw":[0.10148234537109617,0.01825326608643147,-0.06394119106949898,0.9926125507668002],"translation_xyz_m":[0.04833477553308473,0.06049147393224069,0.6629762179810317],"reprojection_error_px":0.42256575477532776}]}]},{"stamp_ns":1789379941645994864,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.052986387193529896}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1957333452543057,0.04677635469777468,-0.04961103680983746,0.9782837907393742],"translation_xyz_m":[0.04762895563001285,0.0605134876175877,0.6630303636820268],"reprojection_error_px":0.1672867645854959},{"quaternion_xyzw":[0.10221236945851817,0.018802067680306466,-0.05899802614881567,0.9928334939410356],"translation_xyz_m":[0.0476338802475757,0.060563206682770446,0.6629842381944319],"reprojection_error_px":0.42201025224548033}]}]},{"stamp_ns":1789379941679359069,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07291500889333562}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19619434033501326,0.046086214816257354,-0.044538223787757264,0.9784682867861638],"translation_xyz_m":[0.04692164007091566,0.06055979256338887,0.6629848403703461],"reprojection_error_px":0.1409426563417575},{"quaternion_xyzw":[0.10299314631062355,0.018933134519222742,-0.05384896631677437,0.9930429180337224],"translation_xyz_m":[0.046926313302071074,0.060609920395682394,0.662940433789015],"reprojection_error_px":0.4040373519840538}]}]},{"stamp_ns":1789379941712615547,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.053139565983862845}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19638660711443195,0.045607388767877954,-0.04010395926744698,0.9786439286517504],"translation_xyz_m":[0.04624619728076382,0.06061101219119662,0.6629865219582689],"reprojection_error_px":0.16901348338326602},{"quaternion_xyzw":[0.10345519861692182,0.018824029897068273,-0.04930055770419787,0.9932331714092228],"translation_xyz_m":[0.04625044356674897,0.060661070513999264,0.6629398107575992],"reprojection_error_px":0.4255691044378717}]}]},{"stamp_ns":1789379941746230604,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.056393358069490515}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1969610304224262,0.043940406644490365,-0.03503592953188857,0.9787993036372111],"translation_xyz_m":[0.04552283278612164,0.06065710549657598,0.6629674908778886],"reprojection_error_px":0.1742876422744853},{"quaternion_xyzw":[0.10433853758608082,0.019889721220865426,-0.04424339425246334,0.9933581381501716],"translation_xyz_m":[0.04552632492326999,0.06070740952382239,0.6629202468825706],"reprojection_error_px":0.4303560932247575}]}]},{"stamp_ns":1789379941779503646,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0453363103473883}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1974167732578148,0.043344966874019786,-0.030119406220121193,0.9788976723090673],"translation_xyz_m":[0.044831077218953984,0.06069582071561116,0.6629092701927387],"reprojection_error_px":0.17096557871248855},{"quaternion_xyzw":[0.10509306335532922,0.019927232000200346,-0.039240562959322434,0.9934880631784352],"translation_xyz_m":[0.044834135806998444,0.06074622781427051,0.6628605893964815],"reprojection_error_px":0.43569985671204153}]}]},{"stamp_ns":1789379941812691639,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0741531381727082}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19792828806329713,0.04214280262195423,-0.024938637304544174,0.9789925644972447],"translation_xyz_m":[0.04407804704404847,0.06074450211665915,0.6629449250282482],"reprojection_error_px":0.17251342160729655},{"quaternion_xyzw":[0.10590732227447622,0.020488987992679463,-0.03401168437327012,0.9935829335217967],"translation_xyz_m":[0.04408064566537307,0.060795286442544755,0.6628976174860837],"reprojection_error_px":0.4300795248096362}]}]},{"stamp_ns":1789379941846036459,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0533412208377475}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1980535514073168,0.04119472419766364,-0.019906963980833768,0.9791228208239727],"translation_xyz_m":[0.04338440853955879,0.060771669839667294,0.6631077537221243],"reprojection_error_px":0.16174667655566896},{"quaternion_xyzw":[0.10636301207362878,0.020868633424316104,-0.028899354092066565,0.9936881991523739],"translation_xyz_m":[0.043386526370462544,0.060822562498851776,0.6630603666691279],"reprojection_error_px":0.4263200785601123}]}]},{"stamp_ns":1789379941879568163,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08348428199629089}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1979359679505185,0.04139143996885696,-0.015189134444307648,0.9792228507768946],"translation_xyz_m":[0.04266446361279267,0.060809098471325075,0.6632081142494896],"reprojection_error_px":0.17122711128416576},{"quaternion_xyzw":[0.10652705165247466,0.0200417139178047,-0.023969550168184826,0.9938187851083328],"translation_xyz_m":[0.04266649063287918,0.060859930523835794,0.663159864697731],"reprojection_error_px":0.4338800625349074}]}]},{"stamp_ns":1789379941913108259,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05866039522408322}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19778021366331425,0.04009445866503941,-0.01030006790246286,0.9793719059013567],"translation_xyz_m":[0.041961899861911636,0.06081338109852576,0.6632640083556435],"reprojection_error_px":0.2106781042096541},{"quaternion_xyzw":[0.10669234869929846,0.020739878500996907,-0.019009004659539572,0.9938939872595289],"translation_xyz_m":[0.041963255991518705,0.06086412264502904,0.6632144439693783],"reprojection_error_px":0.45682484400437756}]}]},{"stamp_ns":1789379941946031464,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05062724117176867}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1979138871984518,0.038604161308568724,-0.004969386372064449,0.9794463319563484],"translation_xyz_m":[0.0412314110800927,0.06081621391966316,0.6632650603450689],"reprojection_error_px":0.2407930137272594},{"quaternion_xyzw":[0.10716425524348185,0.02163265658363264,-0.013637831552080492,0.9939124006258306],"translation_xyz_m":[0.041232006521020055,0.06086696058516038,0.6632141686817472],"reprojection_error_px":0.47721584933661376}]}]},{"stamp_ns":1789379941979528372,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.060339771410208694}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19925013346916212,0.038489440206336815,0.00016396154930296947,0.9791924838466307],"translation_xyz_m":[0.04050667374775337,0.06078059965436906,0.6628894579664273],"reprojection_error_px":0.17097986503669246},{"quaternion_xyzw":[0.10883455083535168,0.021188086553920713,-0.008423027575963728,0.9937983488309413],"translation_xyz_m":[0.040507347683139655,0.06083226273618438,0.6628427745133371],"reprojection_error_px":0.42707137634060055}]}]},{"stamp_ns":1789379942012795085,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.10063657941611068}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.19818854191977012,0.03659959146380564,0.0038618298649074406,0.9794727449125333],"translation_xyz_m":[0.03988270729392319,0.06080856702592379,0.6634167735447487],"reprojection_error_px":0.17167746683110469},{"quaternion_xyzw":[0.10801923785894062,0.02246304954575448,-0.004662884101080882,0.9938840541880821],"translation_xyz_m":[0.0398823838110869,0.06085951491405095,0.6633658371681511],"reprojection_error_px":0.4468787806315853}]}]},{"stamp_ns":1789379942046088606,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.050668177029829675}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20003087879589107,0.038332379780412665,0.008753388784999924,0.9790003342048932],"translation_xyz_m":[0.03916992758469348,0.06076637568660862,0.6629214748395618],"reprojection_error_px":0.1477595418160022},{"quaternion_xyzw":[0.11017705836480356,0.020163155153735707,0.0004511787188801545,0.993707330868646],"translation_xyz_m":[0.039170103571042815,0.060818023861540546,0.6628706260464697],"reprojection_error_px":0.43756444448126924}]}]},{"stamp_ns":1789379942079468490,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.046558075590810016}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1994888406843565,0.03589618677104336,0.013814484506436561,0.9791449464892985],"translation_xyz_m":[0.038497179270408806,0.06075337494755443,0.6631884952865964],"reprojection_error_px":0.14758858824575607},{"quaternion_xyzw":[0.1099675485141005,0.022042940194085786,0.005497019400393355,0.9936755153666164],"translation_xyz_m":[0.03849647155462238,0.06080510790000291,0.663139958630202],"reprojection_error_px":0.4272626884678498}]}]},{"stamp_ns":1789379942112644602,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04887765976384986}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.1994680600173676,0.03605101601093701,0.018488737651129662,0.9790663837848559],"translation_xyz_m":[0.03781214998462917,0.06072474673971475,0.663171607628839],"reprojection_error_px":0.16180010419493362},{"quaternion_xyzw":[0.11025391795650405,0.021303776852769605,0.010363557107562088,0.9936210642650075],"translation_xyz_m":[0.037811278163091135,0.060776374169629964,0.6631212859400126],"reprojection_error_px":0.4404787698584441}]}]},{"stamp_ns":1789379942146008855,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05245233368987363}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2005995460133511,0.03459850421045839,0.023406393495605665,0.9787823590508592],"translation_xyz_m":[0.03712719890198499,0.06068135908497214,0.6630599666221092],"reprojection_error_px":0.18469612720152812},{"quaternion_xyzw":[0.11171312631542103,0.022175449997196468,0.015261669548040022,0.9933758142157796],"translation_xyz_m":[0.03712571432598165,0.06073348863159469,0.6630104833773348],"reprojection_error_px":0.4456935556327976}]}]},{"stamp_ns":1789379942179364213,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06799292124786259}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20093795475664972,0.03353358982484139,0.028210417962440133,0.9786233233527893],"translation_xyz_m":[0.036406571031903444,0.06062664365136486,0.6630877314641077],"reprojection_error_px":0.1603510315353451},{"quaternion_xyzw":[0.11239286576754753,0.022609507863178908,0.02013707611678511,0.9932024728343207],"translation_xyz_m":[0.03640458003758898,0.060678929742774765,0.6630381726704698],"reprojection_error_px":0.4366731907211584}]}]},{"stamp_ns":1789379942212673025,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04822125460561814}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2006464340335789,0.03299965498925196,0.03295126125769413,0.9785531389054588],"translation_xyz_m":[0.035720409597947485,0.06059217576114309,0.6631700338179672],"reprojection_error_px":0.18015199566665463},{"quaternion_xyzw":[0.11240438971700036,0.022548357722237893,0.02502680621310506,0.9930913773199073],"translation_xyz_m":[0.03571804784827465,0.06064431483963877,0.6631193757440093],"reprojection_error_px":0.44927576956598864}]}]},{"stamp_ns":1789379942246296945,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05318925830001911}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20109099641490033,0.03227857614449896,0.037902327458527736,0.9783066585974834],"translation_xyz_m":[0.0350346363899918,0.06052736522010539,0.6631462315477131],"reprojection_error_px":0.16129987213759583},{"quaternion_xyzw":[0.11319511724628491,0.022702328319309212,0.03006483557095778,0.9928582856493535],"translation_xyz_m":[0.035031834844919583,0.060579639294707756,0.6630948778587852],"reprojection_error_px":0.44527433042111536}]}]},{"stamp_ns":1789379942279391479,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.052663321214573594}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20106888213257543,0.031320905110819065,0.042470878662410574,0.9781546554643791],"translation_xyz_m":[0.034369972773527546,0.06046137315858321,0.6631699611791635],"reprojection_error_px":0.17834916465045955},{"quaternion_xyzw":[0.1134914072915443,0.023076162834244216,0.034723295666943724,0.992663832280525],"translation_xyz_m":[0.03436671563864043,0.06051369025956629,0.6631186711823761],"reprojection_error_px":0.4515709298950475}]}]},{"stamp_ns":1789379942313183102,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07255293831275442}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20145238167463714,0.030871383685179332,0.04715082885086978,0.9778756029913288],"translation_xyz_m":[0.033682667211330015,0.060391436379941085,0.6631729936526723],"reprojection_error_px":0.1817352822681532},{"quaternion_xyzw":[0.11420162999858216,0.022920900049701192,0.03952420106731449,0.9924063973879698],"translation_xyz_m":[0.03367908744038761,0.06044385673858307,0.66312094453485],"reprojection_error_px":0.45621954848114277}]}]},{"stamp_ns":1789379942346045331,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09207843563501118}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20160107285207787,0.029911634166625012,0.05192948215926762,0.9776326664187545],"translation_xyz_m":[0.03300603264756638,0.06030970004677269,0.6631942611099345],"reprojection_error_px":0.18204943758391262},{"quaternion_xyzw":[0.11468775427760884,0.023295906915475104,0.04438694777739726,0.9921359879606984],"translation_xyz_m":[0.03300205230832117,0.060362343764376505,0.6631436943322162],"reprojection_error_px":0.44998187265481954}]}]},{"stamp_ns":1789379942379396002,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0428275705657556}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20182483294773254,0.028740255070283903,0.056827467470331135,0.9773491563842593],"translation_xyz_m":[0.03231019498132332,0.06023384982138472,0.663165183320836],"reprojection_error_px":0.1758564320059407},{"quaternion_xyzw":[0.11523422302036992,0.023869610539792918,0.0493513814591113,0.991824458604267],"translation_xyz_m":[0.03230563839179417,0.060286562874551405,0.6631142249108025],"reprojection_error_px":0.44943994177503277}]}]},{"stamp_ns":1789379942412891190,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06973563752386505}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20220646622461663,0.02844712308251257,0.06139899533389537,0.9770022873961297],"translation_xyz_m":[0.031643177910870166,0.06014827749971275,0.66318256617681],"reprojection_error_px":0.17935829774559314},{"quaternion_xyzw":[0.11594126322735604,0.023571786736276214,0.05405594220864292,0.9915038827272641],"translation_xyz_m":[0.031638336442465276,0.06020105268224904,0.663130459271582],"reprojection_error_px":0.45579647942313645}]}]},{"stamp_ns":1789379942446488169,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08639265205665099}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20237911445045803,0.027386133327980376,0.06613425103735997,0.9766877467109526],"translation_xyz_m":[0.030967861948019895,0.060056901138867726,0.6632090892928917],"reprojection_error_px":0.1710726478781151},{"quaternion_xyzw":[0.11644467688836417,0.02404398893520551,0.05886694339741377,0.9911595264111089],"translation_xyz_m":[0.030962472388509906,0.060109685551847106,0.6631561798070064],"reprojection_error_px":0.45625279359413834}]}]},{"stamp_ns":1789379942479942800,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04881143459337717}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2023386992622669,0.026671403276442237,0.07088647917781232,0.9763825039900567],"translation_xyz_m":[0.030299130748557394,0.059954500884265266,0.6632299796111686],"reprojection_error_px":0.1713205521455187},{"quaternion_xyzw":[0.11674067460966699,0.02417796877983837,0.06373525297340886,0.9908202956367904],"translation_xyz_m":[0.030293377622390958,0.060007350274928456,0.6631775480667611],"reprojection_error_px":0.4543918378978383}]}]},{"stamp_ns":1789379942512750474,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07293400506973971}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2027627270814897,0.026179793383333323,0.07589667250068462,0.975931140003334],"translation_xyz_m":[0.02957961398847826,0.05982986361488528,0.663204637580027],"reprojection_error_px":0.1807180171257879},{"quaternion_xyzw":[0.11753792612038387,0.024031433730717876,0.06887491203285254,0.9903855676496058],"translation_xyz_m":[0.029573468410268505,0.05988274441512508,0.6631505523416676],"reprojection_error_px":0.4651156849634616}]}]},{"stamp_ns":1789379942546336373,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04433491756923311}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20268693185738482,0.024835365998192204,0.08086309391904639,0.9755830934840021],"translation_xyz_m":[0.028879666108564045,0.05971376498469641,0.6632653671637054],"reprojection_error_px":0.18081071484673053},{"quaternion_xyzw":[0.11781202328273306,0.024756386076124546,0.07391321903700634,0.9899718604940577],"translation_xyz_m":[0.02887298117879546,0.05976678398133672,0.6632129633053783],"reprojection_error_px":0.4581263201060898}]}]},{"stamp_ns":1789379942579618846,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0632026514060978}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2030132215648616,0.024230587433593843,0.08582505589692897,0.9751064404888158],"translation_xyz_m":[0.02816927197205471,0.05957979364209356,0.6631886252759561],"reprojection_error_px":0.17058819858271757},{"quaternion_xyzw":[0.11849538801832028,0.02473522935722533,0.07899746296427938,0.9894980607824557],"translation_xyz_m":[0.02816225551626228,0.059633015560882796,0.6631368939401974],"reprojection_error_px":0.45137466153630995}]}]},{"stamp_ns":1789379942613068987,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.055226570300284765}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20301270198575905,0.02320357347651032,0.09058818488870007,0.974700578520877],"translation_xyz_m":[0.0274963943724691,0.05945409439260527,0.6633180831290427],"reprojection_error_px":0.1865917105259929},{"quaternion_xyzw":[0.11885231765956973,0.025154227058757094,0.08385279650432825,0.9890450444577481],"translation_xyz_m":[0.027488774548161628,0.05950709041173178,0.663263747339594],"reprojection_error_px":0.46883111686159457}]}]},{"stamp_ns":1789379942646452726,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05797637150786821}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20342747467855304,0.022037592298741544,0.09556907969147191,0.9741653648526549],"translation_xyz_m":[0.02679289688303413,0.05929827889248816,0.663190566201625],"reprojection_error_px":0.19405236950355093},{"quaternion_xyzw":[0.11963477146994762,0.02569334237820974,0.08890320461063167,0.9884956215495998],"translation_xyz_m":[0.026784762199774836,0.05935153640885972,0.6631374580510339],"reprojection_error_px":0.4668489430694782}]}]},{"stamp_ns":1789379942679642301,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05980446959507774}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20361860284776104,0.021039277227882434,0.0998598217145706,0.9737170171026016],"translation_xyz_m":[0.02619626097176933,0.05917659094430265,0.6631995650165952],"reprojection_error_px":0.18796789539880068},{"quaternion_xyzw":[0.12013346576769675,0.026163451007928294,0.09325988866532076,0.9880212636376452],"translation_xyz_m":[0.026187634033405374,0.0592298901354789,0.6631462211355788],"reprojection_error_px":0.46548878041587255}]}]},{"stamp_ns":1789379942713141262,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0527726572069157}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20361097274351264,0.020779312982973638,0.10406965964842803,0.9732832567506011],"translation_xyz_m":[0.02558997960183743,0.05905623134692422,0.6633381612517109],"reprojection_error_px":0.17273114127423728},{"quaternion_xyzw":[0.12043506139570385,0.025871589725302943,0.09761386350105054,0.9875715622090872],"translation_xyz_m":[0.025581132315223302,0.05910950887227563,0.6632843902955285],"reprojection_error_px":0.4613694805255691}]}]},{"stamp_ns":1789379942746573462,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.046993453199584825}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20366770896024305,0.019892379119722945,0.10867526611080608,0.972786432941778],"translation_xyz_m":[0.02493532766796895,0.0589036761071662,0.6633470940646669],"reprojection_error_px":0.1933246184104725},{"quaternion_xyzw":[0.12083964986563116,0.026159326155570605,0.10231950749401907,0.9870380879487982],"translation_xyz_m":[0.02492595882966938,0.058956792227452655,0.663291197905334],"reprojection_error_px":0.4784138230507348}]}]},{"stamp_ns":1789379942779717995,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.061523367205298836}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20383561439700992,0.019005267239628747,0.11177629873715239,0.9724175549429199],"translation_xyz_m":[0.024505964567285633,0.058798063146276044,0.6633790504038514],"reprojection_error_px":0.19034074701145873},{"quaternion_xyzw":[0.12124509576469615,0.026657482948467055,0.10545417413628465,0.9866450336940311],"translation_xyz_m":[0.02449621420329299,0.05885128291768867,0.6633238357116861],"reprojection_error_px":0.4744633579389717}]}]},{"stamp_ns":1789379942812935192,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0632466378650773}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20401483509900498,0.01870410585548885,0.11474958838580668,0.9720394207278635],"translation_xyz_m":[0.02407873108470468,0.058694586823836443,0.6633320058532415],"reprojection_error_px":0.1885165429694467},{"quaternion_xyzw":[0.12164267249162447,0.026569834655640587,0.1085119113600405,0.9862668346895064],"translation_xyz_m":[0.024068771175360323,0.05874784053497336,0.6632763467337747],"reprojection_error_px":0.4756444378286271}]}]},{"stamp_ns":1789379942846193057,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05277053538978022}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20399324539340533,0.018477736495826604,0.11723928738165307,0.9717511402525334],"translation_xyz_m":[0.023727121397393417,0.058599971444251414,0.6633516098962364],"reprojection_error_px":0.19988257907022425},{"quaternion_xyzw":[0.12182165430421175,0.02647239715115463,0.11108012130744778,0.9859614106961689],"translation_xyz_m":[0.023716948273443735,0.058653069215017145,0.6632941010665429],"reprojection_error_px":0.48782759713092344}]}]},{"stamp_ns":1789379942879864955,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06135060978871932}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20373152927897717,0.01791722367461674,0.11994578036334175,0.9714862051755918],"translation_xyz_m":[0.023361158112361734,0.05852145560723774,0.6634539190975288],"reprojection_error_px":0.19484076991082236},{"quaternion_xyzw":[0.12174176010072622,0.026704034010018862,0.11384899518487748,0.9856491488915123],"translation_xyz_m":[0.023350822679810375,0.05857471858260598,0.6633993498977012],"reprojection_error_px":0.4737246555132505}]}]},{"stamp_ns":1789379942913121984,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07989264200394498}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20408398696257993,0.017581551398795487,0.12260722485928933,0.9710860331238282],"translation_xyz_m":[0.02297878287928909,0.058396111930380946,0.663370425717349],"reprojection_error_px":0.19584380353207761},{"quaternion_xyzw":[0.12232755772180166,0.026683843763304363,0.11657398169976939,0.9852585690540817],"translation_xyz_m":[0.022968235799406163,0.05844948778551782,0.6633156477168],"reprojection_error_px":0.4750570880374823}]}]},{"stamp_ns":1789379942946246963,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.053482522970155336}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20417593007753945,0.016908414705415844,0.12609255875413422,0.9706322484421979],"translation_xyz_m":[0.022483132456031923,0.0582794674501893,0.6634189505720349],"reprojection_error_px":0.19919843134171938},{"quaternion_xyzw":[0.12266791569112334,0.02689772033601905,0.12013587190720111,0.9847824467271019],"translation_xyz_m":[0.022472208233457053,0.05833275579624108,0.6633629451483605],"reprojection_error_px":0.481550940055561}]}]},{"stamp_ns":1789379942979558196,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.053904990736433976}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20433474059162926,0.016325398029387442,0.1275210982897639,0.9704221579588473],"translation_xyz_m":[0.0222936236932614,0.05822164608989357,0.6634224215022998],"reprojection_error_px":0.2064355556753102},{"quaternion_xyzw":[0.12294318851422452,0.027306878467824532,0.12156109392317883,0.9845619367162735],"translation_xyz_m":[0.022282444537649816,0.05827496741654663,0.6633663980018539],"reprojection_error_px":0.4846908297839301}]}]},{"stamp_ns":1789379943012920306,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04595723225596542}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20401046941291773,0.015997569892927346,0.12933611034125395,0.9702556244048456],"translation_xyz_m":[0.0220456875758332,0.058156389606534346,0.6634384397439091],"reprojection_error_px":0.20736918314006125},{"quaternion_xyzw":[0.12274717446909685,0.02741190741421237,0.12342746185506381,0.9843512483619781],"translation_xyz_m":[0.022034356182372237,0.05820961787271891,0.6633825352684167],"reprojection_error_px":0.48471137271739073}]}]},{"stamp_ns":1789379943046257233,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.054230823786852865}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20426778411618188,0.014791237259813326,0.13303690435944082,0.9697201007254855],"translation_xyz_m":[0.02153969575781024,0.05801280871248912,0.6634026176530917],"reprojection_error_px":0.20617508001637716},{"quaternion_xyzw":[0.12328248069963577,0.0281578420592256,0.1271552036992432,0.983788656193654],"translation_xyz_m":[0.021527796716160386,0.05806602387371689,0.6633459701870421],"reprojection_error_px":0.4873840214014774}]}]},{"stamp_ns":1789379943079777307,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04805963234391007}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20436456625696423,0.015337983429525146,0.13522203661871968,0.9693889163465839],"translation_xyz_m":[0.021229005214908135,0.05790614569878843,0.6634358977823118],"reprojection_error_px":0.2088907861150165},{"quaternion_xyzw":[0.12357833716115288,0.02731725177716725,0.12947238588741028,0.9834729602955278],"translation_xyz_m":[0.021217226292378353,0.05795941022007012,0.6633789120903401],"reprojection_error_px":0.4897815687863677}]}]},{"stamp_ns":1789379943113094924,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.059928646959367775}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20422733791322623,0.014447023174114865,0.13746270471782407,0.9691163412005516],"translation_xyz_m":[0.020900236642322486,0.05783210924360685,0.6634396377250633],"reprojection_error_px":0.20993718830375263},{"quaternion_xyzw":[0.12357986146809546,0.02789682698101104,0.13173305848443073,0.9831562369157077],"translation_xyz_m":[0.020888106572764433,0.05788533809000313,0.6633831079021592],"reprojection_error_px":0.4885300046634141}]}]},{"stamp_ns":1789379943146411692,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06560679257145154}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20423691635122193,0.01368450300644613,0.1397250190661474,0.9688018039949013],"translation_xyz_m":[0.020584650650217917,0.057728192779678174,0.6634744656925068],"reprojection_error_px":0.20399749932013878},{"quaternion_xyzw":[0.12377913871337295,0.028361662887500676,0.13401776771935064,0.9828090245989571],"translation_xyz_m":[0.02057221576493049,0.05778147810041679,0.6634187944500703],"reprojection_error_px":0.4826012434398359}]}]},{"stamp_ns":1789379943179734049,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.044727780550153945}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20473145237280507,0.014182667144818265,0.1418433022062606,0.9683823428693878],"translation_xyz_m":[0.020284798500639162,0.057632930003608605,0.6634407325347893],"reprojection_error_px":0.21844337458117946},{"quaternion_xyzw":[0.12444960209919151,0.027579838218870303,0.1362488832733446,0.9824397644985369],"translation_xyz_m":[0.02027239985360459,0.057686241824906365,0.663383071043845],"reprojection_error_px":0.4966798271205632}]}]},{"stamp_ns":1789379943213292799,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06744347875998982}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20472595622369918,0.013575808926834087,0.14289739747750907,0.9682372199282719],"translation_xyz_m":[0.020159698099627123,0.05758692263019785,0.6634799769583625],"reprojection_error_px":0.21303816436117648},{"quaternion_xyzw":[0.12453454548770874,0.028078570193338356,0.13728540382171953,0.9822705629171501],"translation_xyz_m":[0.02014706118329792,0.05764023786356961,0.6634226787919157],"reprojection_error_px":0.4929473403808454}]}]},{"stamp_ns":1789379943246262297,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06788833141219228}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20494295313662952,0.013061416032586584,0.14447887684801522,0.9679636560923126],"translation_xyz_m":[0.01993294768155995,0.05750794073583843,0.6634039409503922],"reprojection_error_px":0.21560592060707262},{"quaternion_xyzw":[0.12488240794462314,0.028378078094140273,0.13887950470563945,0.9819936619155705],"translation_xyz_m":[0.01992004633542454,0.057561227262714135,0.6633456268208687],"reprojection_error_px":0.49819762766322667}]}]},{"stamp_ns":1789379943279881076,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05156031273320985}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20509712495027038,0.012987183294790716,0.1455015150162109,0.9677788029995221],"translation_xyz_m":[0.019796232310472808,0.05746570111319624,0.6634179511163226],"reprojection_error_px":0.20453413891220099},{"quaternion_xyzw":[0.12511428139325712,0.028328007626713992,0.13992961314041286,0.981816502174273],"translation_xyz_m":[0.019783323065274524,0.05751919680255444,0.6633614849579363],"reprojection_error_px":0.48604168219073146}]}]},{"stamp_ns":1789379943313239197,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.095129466781501}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2050646042418519,0.012517661871380358,0.1480266596605761,0.9674088712939004],"translation_xyz_m":[0.01943884890317382,0.05736468265428036,0.663396330366552],"reprojection_error_px":0.2202490868038033},{"quaternion_xyzw":[0.1252582944176455,0.028458719621564963,0.14251761692113102,0.9814220243219556],"translation_xyz_m":[0.019425690174724848,0.057418110134415314,0.6633392914532453],"reprojection_error_px":0.4951927412650559}]}]},{"stamp_ns":1789379943346550790,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07070885831250469}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2046474533630123,0.011647188565297305,0.15046177399512417,0.967132368084534],"translation_xyz_m":[0.019102864630292423,0.057259828508536004,0.6635245313592966],"reprojection_error_px":0.227182711896892},{"quaternion_xyzw":[0.12503492008494815,0.029007747934718997,0.14498664327679414,0.9810727254339681],"translation_xyz_m":[0.019089325266178236,0.057313006342104476,0.6634665480094553],"reprojection_error_px":0.5021359356703752}]}]},{"stamp_ns":1789379943379942702,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.051436433605660886}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2045657901288246,0.01194505756517242,0.15075563169308098,0.9671002495199525],"translation_xyz_m":[0.01906504670906489,0.05724780293264717,0.6634898026145343],"reprojection_error_px":0.22633718509496897},{"quaternion_xyzw":[0.12497138645981039,0.02868139597647473,0.1453184220377207,0.9810413275227605],"translation_xyz_m":[0.01905157281854115,0.05730087980441432,0.6634306986690973],"reprojection_error_px":0.5061621524933896}]}]},{"stamp_ns":1789379943413013062,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.048358556460066}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20493656285596693,0.01175017418643089,0.15259041928045725,0.9667363149045273],"translation_xyz_m":[0.018799218258880365,0.05716259981378679,0.6634845142036808],"reprojection_error_px":0.22168811836318877},{"quaternion_xyzw":[0.12548806627479037,0.02861904991112021,0.1472017441803183,0.9806963555122813],"translation_xyz_m":[0.018785630843860545,0.05721584985708946,0.663426060828548],"reprojection_error_px":0.501503453002103}]}]},{"stamp_ns":1789379943446791873,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06029381580446954}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20508648688829387,0.011132240070775677,0.1535785701539594,0.9665553418797547],"translation_xyz_m":[0.018672243460206184,0.05708825192469589,0.6634544508855033],"reprojection_error_px":0.2141633117950894},{"quaternion_xyzw":[0.12575657118178946,0.029116755284572367,0.1481680851908961,0.9805017684314298],"translation_xyz_m":[0.018658419650215974,0.05714158871543778,0.6633966776088059],"reprojection_error_px":0.49562944964033284}]}]},{"stamp_ns":1789379943479839227,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05186254383345303}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20512977737394011,0.011267329016010344,0.1555690473457446,0.9662262122501647],"translation_xyz_m":[0.018396223854118668,0.05701284140155992,0.6634731666225209],"reprojection_error_px":0.20777629808956655},{"quaternion_xyzw":[0.12593130832955626,0.02872565428608094,0.15024782968617123,0.9801743375761468],"translation_xyz_m":[0.018382429922294708,0.05706633139779858,0.6634168452255147],"reprojection_error_px":0.48701533969682276}]}]},{"stamp_ns":1789379943513056309,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05505894022330594}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20499432026788106,0.011017817666727218,0.155996381324361,0.9661889387513587],"translation_xyz_m":[0.018334738034223315,0.05698002973632729,0.6634989318874968],"reprojection_error_px":0.2179949264874673},{"quaternion_xyzw":[0.12585063241398692,0.028909574448371493,0.1506757846961678,0.9801135968511036],"translation_xyz_m":[0.01832081671208804,0.05703335795129335,0.663441312600954],"reprojection_error_px":0.49667057312986146}]}]},{"stamp_ns":1789379943546704679,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.040564817672389175}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20496319437934904,0.010558287648418662,0.15605181707288723,0.9661917210885201],"translation_xyz_m":[0.018335505256795993,0.056995783107213614,0.6635473661756918],"reprojection_error_px":0.21841855397096624},{"quaternion_xyzw":[0.1258038435041712,0.029373804466058258,0.15069272439543058,0.9801031962936609],"translation_xyz_m":[0.018321423002241505,0.057049072113410135,0.6634898136573788],"reprojection_error_px":0.4966143874755629}]}]},{"stamp_ns":1789379943579868673,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.044150032529922785}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20502781231417413,0.010996330936096059,0.15774042832979537,0.9658988736684293],"translation_xyz_m":[0.018091826958388013,0.05691497488429571,0.6635183110372439],"reprojection_error_px":0.2076125790989879},{"quaternion_xyzw":[0.12599694196670425,0.0287064848357319,0.15248616091669176,0.9798207382334299],"translation_xyz_m":[0.0180778678966439,0.05696838118418809,0.6634614330491692],"reprojection_error_px":0.4892247430314572}]}]},{"stamp_ns":1789379943613404344,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09210874308741177}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20494263297874096,0.01022382609618209,0.15861655364275665,0.9657819523464862],"translation_xyz_m":[0.01797281835176121,0.05686726589761063,0.6634771837034454],"reprojection_error_px":0.21861173148812832},{"quaternion_xyzw":[0.1259878086284102,0.029366225728335117,0.15333218901159473,0.9796703204019177],"translation_xyz_m":[0.017958482179687856,0.056920346003491135,0.663417212037284],"reprojection_error_px":0.5063527129892088}]}]},{"stamp_ns":1789379943646650815,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0635535759637431}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20477140372135852,0.010041204339541572,0.15872140692439163,0.9658029620048479],"translation_xyz_m":[0.01796349486926509,0.05686480120664262,0.6634923172360774],"reprojection_error_px":0.21590716564513218},{"quaternion_xyzw":[0.12582001533935736,0.029542353271927824,0.1534288301655581,0.9796714587948293],"translation_xyz_m":[0.017949131238859047,0.05691792112986602,0.6634335646336591],"reprojection_error_px":0.500452631980323}]}]},{"stamp_ns":1789379943679787737,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05933083273614831}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20511813330175405,0.010129455060690152,0.15996990450106932,0.9655224363964289],"translation_xyz_m":[0.01779646639075722,0.056803907320759024,0.6634394658302637],"reprojection_error_px":0.22896291689708287},{"quaternion_xyzw":[0.1262662724570052,0.029295580903935754,0.15472380135020486,0.979417756973528],"translation_xyz_m":[0.017782088017290632,0.05685717467071684,0.6633809891364312],"reprojection_error_px":0.5050585873574194}]}]},{"stamp_ns":1789379943713246593,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.09553229950415702}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20507185804696756,0.010134693463916232,0.16117443161458753,0.9653318722694655],"translation_xyz_m":[0.01762359372309195,0.05674695825458654,0.6635109324232361],"reprojection_error_px":0.23927869233302249},{"quaternion_xyzw":[0.1263199003065173,0.029116665521832195,0.1559810495272743,0.9792167353368769],"translation_xyz_m":[0.017609189462366007,0.056800212566887415,0.6634524203813023],"reprojection_error_px":0.5098690369842988}]}]},{"stamp_ns":1789379943746638861,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05927096247589221}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20516275075172383,0.00998956049769469,0.16135344621780118,0.9652841652998811],"translation_xyz_m":[0.017609074747326762,0.05673762982912756,0.6634916464959616],"reprojection_error_px":0.22577846377848382},{"quaternion_xyzw":[0.12642654488459143,0.029257137812663293,0.15614819449046832,0.979172145229286],"translation_xyz_m":[0.017594610848997737,0.056790922666448515,0.6634333361234748],"reprojection_error_px":0.502945682327819}]}]},{"stamp_ns":1789379943779955665,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04671991595503438}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20501146130011907,0.009993748267567812,0.16133054206781422,0.9653200929884653],"translation_xyz_m":[0.017599832997837357,0.05673382332034285,0.6635168157214305],"reprojection_error_px":0.2358507295607526},{"quaternion_xyzw":[0.12627960637702892,0.029233571042499275,0.15613339411054605,0.9791941700096497],"translation_xyz_m":[0.017585370147933183,0.056787026395096744,0.6634579915200695],"reprojection_error_px":0.5095226496196581}]}]},{"stamp_ns":1789379943813769027,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06603590695656815}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20500864676387148,0.009952798044769166,0.1613332979532169,0.965320653220817],"translation_xyz_m":[0.017598206342162102,0.056725119245553776,0.6635142799933547],"reprojection_error_px":0.2338102551529954},{"quaternion_xyzw":[0.12628914303827038,0.02927118152649845,0.15613307065882429,0.9791918680878373],"translation_xyz_m":[0.017583723758157397,0.05677830363905221,0.6634552425189851],"reprojection_error_px":0.5094275199730773}]}]},{"stamp_ns":1789379943846758288,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05666379340999475}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20491498006025127,0.01005705163935952,0.16134655125796898,0.9653372452445775],"translation_xyz_m":[0.017597538034503778,0.056744301752316595,0.6635296073616211],"reprojection_error_px":0.23947991844185498},{"quaternion_xyzw":[0.12616912600888117,0.0291702783943316,0.15615812275588264,0.9792063557789598],"translation_xyz_m":[0.0175830844670274,0.056797418948313846,0.6634701270059455],"reprojection_error_px":0.5137310175011051}]}]},{"stamp_ns":1789379943880258212,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05777691527926094}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20516211334357354,0.010105205051442353,0.16135465940367902,0.96528289426882],"translation_xyz_m":[0.017595173071748554,0.05672353180913763,0.6635075250711082],"reprojection_error_px":0.23430847857440534},{"quaternion_xyzw":[0.12644681135054758,0.02911582511022697,0.15616416710497427,0.9791711931730015],"translation_xyz_m":[0.017580728661652428,0.05677674955276746,0.6634482108085398],"reprojection_error_px":0.5106789116331977}]}]},{"stamp_ns":1789379943913439299,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04409581271162895}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2051966232325764,0.01002624042410698,0.1613325094467074,0.9652800845933444],"translation_xyz_m":[0.0175985726710904,0.05673420369891366,0.6634328444174923],"reprojection_error_px":0.23662186903139382},{"quaternion_xyzw":[0.12645758078242783,0.029203927661012596,0.15613278109612164,0.9791721838057985],"translation_xyz_m":[0.01758408939217804,0.05678740260548534,0.6633732262582274],"reprojection_error_px":0.512973766700265}]}]},{"stamp_ns":1789379943946732447,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04451632067500778}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20503916381033915,0.010085925340729768,0.16136576485698712,0.9653073631471469],"translation_xyz_m":[0.01759819451585914,0.05673068090441514,0.6634793587347237],"reprojection_error_px":0.23578806824550141},{"quaternion_xyzw":[0.12630910442994514,0.02914361814044223,0.15617576640919834,0.9791862895518343],"translation_xyz_m":[0.017583755977865333,0.05678388968314241,0.6634204020180038],"reprojection_error_px":0.5100219695698694}]}]},{"stamp_ns":1789379943980046633,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0483483699582677}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20516239434834466,0.010133971716171182,0.16135794038352397,0.9652819845193928],"translation_xyz_m":[0.01760007429115512,0.05673467185856142,0.6635059342598331],"reprojection_error_px":0.23001383578528517},{"quaternion_xyzw":[0.12643072013754633,0.029097868528466832,0.15616818072396024,0.9791731646557569],"translation_xyz_m":[0.01758565912879293,0.056787956677206756,0.6634474263720761],"reprojection_error_px":0.5055875544806263}]}]},{"stamp_ns":1789379944013300851,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06375746607093438}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20525310365241542,0.010118387433165345,0.1613233550364172,0.965268644884209],"translation_xyz_m":[0.01759755835958556,0.05673945700452033,0.6634835073326198],"reprojection_error_px":0.2409819256002233},{"quaternion_xyzw":[0.1265125424767132,0.029106328860183,0.15613108585177723,0.9791682604369413],"translation_xyz_m":[0.017583110561746027,0.056792683948777206,0.6634240031129955],"reprojection_error_px":0.5144772478181137}]}]},{"stamp_ns":1789379944046835441,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06057263026974623}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20532346498412568,0.010108155982610409,0.1613221729613246,0.9652539854466181],"translation_xyz_m":[0.01759580051515155,0.056730769274535346,0.6634638925632614],"reprojection_error_px":0.23725113122905997},{"quaternion_xyzw":[0.12659377367109031,0.029113778945534564,0.15612734973210735,0.9791581358538832],"translation_xyz_m":[0.017581343918283206,0.05678401509588526,0.6634043187156939],"reprojection_error_px":0.5130261961342594}]}]},{"stamp_ns":1789379944080142158,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04528932559341135}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20511090978539687,0.010082650420861643,0.16133190009037893,0.9652978156303501],"translation_xyz_m":[0.017595210279831228,0.05673328899281057,0.6634467774056704],"reprojection_error_px":0.24209239549486902},{"quaternion_xyzw":[0.1263724605265726,0.02914013409553767,0.1561408809824625,0.9791837820814713],"translation_xyz_m":[0.017580764528792883,0.056786502529224306,0.6633876028278071],"reprojection_error_px":0.5137958970090235}]}]},{"stamp_ns":1789379944113632725,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04599716246788295}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20501821997069788,0.009991715764145696,0.1613843877130781,0.9653096780301178],"translation_xyz_m":[0.01759727877128611,0.056736382740708635,0.6635239112047137],"reprojection_error_px":0.23541218562087263},{"quaternion_xyzw":[0.1262864630244907,0.029237501599641835,0.15618725083755,0.9791845793479302],"translation_xyz_m":[0.017582785348644547,0.05678950107883749,0.6634640919281358],"reprojection_error_px":0.5131627152536864}]}]},{"stamp_ns":1789379944146624724,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07185457232585789}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20507240073763164,0.009767081374030734,0.16138747227602962,0.9652999525378063],"translation_xyz_m":[0.017597519314984277,0.05672669796805636,0.6635200423486],"reprojection_error_px":0.23715166415066444},{"quaternion_xyzw":[0.12635470209007724,0.0294586154566443,0.15616991793845666,0.9791719133871238],"translation_xyz_m":[0.017582953877449136,0.05677985254285896,0.6634605746306882],"reprojection_error_px":0.5126278746095436}]}]},{"stamp_ns":1789379944180486437,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.050796262856953066}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.204910699462636,0.009890504997919691,0.1613757285984539,0.9653349974884043],"translation_xyz_m":[0.017597983279363703,0.056733693207044605,0.6635084756059364],"reprojection_error_px":0.2338292846872105},{"quaternion_xyzw":[0.12617843500607526,0.029339460890378324,0.15617254243054463,0.979197802062923],"translation_xyz_m":[0.017583476873627504,0.056786830739984535,0.6634493174457756],"reprojection_error_px":0.5099373498897728}]}]},{"stamp_ns":1789379944213462686,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05622211883337472}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20515140307964322,0.009925233448415005,0.16132987335637985,0.9652911806901909],"translation_xyz_m":[0.017596926995903126,0.05673041277575562,0.6634534658598653],"reprojection_error_px":0.232369470309396},{"quaternion_xyzw":[0.12641845824230943,0.029301034154803923,0.1561232896175847,0.9791758479722016],"translation_xyz_m":[0.01758242428094427,0.056783632571517036,0.6633943272498257],"reprojection_error_px":0.5091875022370166}]}]},{"stamp_ns":1789379944246788283,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05999872706535323}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2052006591580476,0.010147052117165163,0.16132909999599754,0.9652785340558008],"translation_xyz_m":[0.017597347606048093,0.05673588833054715,0.6634383489987754],"reprojection_error_px":0.2374392850537603},{"quaternion_xyzw":[0.1264593012710817,0.029081126402548823,0.1561400903126743,0.9791744509566918],"translation_xyz_m":[0.01758291175297313,0.056789109179926016,0.6633788969438459],"reprojection_error_px":0.5126851234907436}]}]},{"stamp_ns":1789379944280130128,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05023590908180137}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20514705330783664,0.010025898299079981,0.161382298809082,0.9652823014608236],"translation_xyz_m":[0.01759859561700551,0.05672808431205735,0.6634754317067407],"reprojection_error_px":0.23407363273109413},{"quaternion_xyzw":[0.1264228442450255,0.029205110087338115,0.1561839617396591,0.9791684717621831],"translation_xyz_m":[0.01758412190203997,0.05678129908261322,0.66341619084358],"reprojection_error_px":0.5103286865434045}]}]},{"stamp_ns":1789379944313390900,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.042443443555689964}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20532019157796189,0.010259736203304542,0.16136006516983636,0.965246748822193],"translation_xyz_m":[0.017598511305694946,0.05673371575716586,0.6635049955730264],"reprojection_error_px":0.23782467913605063},{"quaternion_xyzw":[0.12659300162374915,0.028967613710449938,0.15617767638522062,0.9791545448470965],"translation_xyz_m":[0.017584111822138895,0.05678698660323182,0.6634456633837376],"reprojection_error_px":0.5123028687890376}]}]},{"stamp_ns":1789379944346898792,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07940652332258286}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20529357515854293,0.009851119149644089,0.1613796861094683,0.965253386609616],"translation_xyz_m":[0.017595373665468997,0.05673843558780692,0.6634359745633963],"reprojection_error_px":0.2414825063020959},{"quaternion_xyzw":[0.1265503151712468,0.02937570189469225,0.15616379674211503,0.9791501184472888],"translation_xyz_m":[0.017580839388453128,0.056791705426046155,0.6633769867348506],"reprojection_error_px":0.5127908560862737}]}]},{"stamp_ns":1789379944380426022,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06270829833114508}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20509706722631177,0.009993447114144667,0.16132134484971256,0.965303448520631],"translation_xyz_m":[0.017597719993269197,0.05672962359940293,0.6635265622759142],"reprojection_error_px":0.22968373875716933},{"quaternion_xyzw":[0.12637349741557682,0.029230544301542167,0.1561221646587705,0.9791839378446986],"translation_xyz_m":[0.017583237522793074,0.05678279981979957,0.663467107778029],"reprojection_error_px":0.509145692050781}]}]},{"stamp_ns":1789379944413532431,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.08254149793364086}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2051750091916523,0.009936888141538227,0.16138961679517075,0.9652760566014139],"translation_xyz_m":[0.017596091298831485,0.05674858707183682,0.6635363807919612],"reprojection_error_px":0.23705463034867413},{"quaternion_xyzw":[0.12642732894257075,0.029290420552427206,0.15618449099549672,0.9791652600724694],"translation_xyz_m":[0.01758160275656526,0.05680184425896959,0.6634777194408397],"reprojection_error_px":0.5093959681302237}]}]},{"stamp_ns":1789379944446818781,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04718600890156761}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20509288608492432,0.010003174838920225,0.16138141978846757,0.9652941944907556],"translation_xyz_m":[0.017598177538282807,0.0567356724847263,0.6635530888614981],"reprojection_error_px":0.235013224812284},{"quaternion_xyzw":[0.12636725398302015,0.0292253196789991,0.1561831630586015,0.9791751719623315],"translation_xyz_m":[0.017583674977970393,0.05678877946323732,0.6634928783798976],"reprojection_error_px":0.5144445918558475}]}]},{"stamp_ns":1789379944480482639,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.07116897570313678}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2050711780063527,0.009700551222160054,0.16135426175102857,0.9653064350100716],"translation_xyz_m":[0.017598305363271494,0.05672610635888451,0.6635139367508445],"reprojection_error_px":0.23115954905046684},{"quaternion_xyzw":[0.12635079869582688,0.02952445967107315,0.15613077476622644,0.9791766761524477],"translation_xyz_m":[0.01758374720984881,0.05677934982090415,0.6634555532603209],"reprojection_error_px":0.5056949065222333}]}]},{"stamp_ns":1789379944513460336,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.0656643862334025}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20511105362129767,0.00994014083588272,0.1613367487432428,0.9652984527012791],"translation_xyz_m":[0.017598413033933145,0.05672587141404727,0.6634957166444954],"reprojection_error_px":0.23354634057519996},{"quaternion_xyzw":[0.1263899658890084,0.02928522612622322,0.15613246188626534,0.979178536528781],"translation_xyz_m":[0.01758391677235731,0.05677907559040081,0.6634365428307946],"reprojection_error_px":0.5098312051264094}]}]},{"stamp_ns":1789379944546833505,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.045827761598395106}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2051904783733093,0.009831784100693762,0.16135523121197423,0.9652795931577856],"translation_xyz_m":[0.01759945554022671,0.056734125842512674,0.663503006937104],"reprojection_error_px":0.23438267081808817},{"quaternion_xyzw":[0.12645950326234298,0.02939651146491728,0.15613943088108265,0.9791651123648484],"translation_xyz_m":[0.01758492955916004,0.056787387318730606,0.6634443348441375],"reprojection_error_px":0.5082525906460021}]}]},{"stamp_ns":1789379944580568532,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.061308204404460546}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2050306925811446,0.009829331862244977,0.1613708852970859,0.9653109533788958],"translation_xyz_m":[0.017599831378844844,0.05674297933124284,0.6635181007622621],"reprojection_error_px":0.239941433214299},{"quaternion_xyzw":[0.12628830994842327,0.02940194832807523,0.15615915317690668,0.9791838985011729],"translation_xyz_m":[0.01758529068962415,0.056796121661006616,0.6634586519084682],"reprojection_error_px":0.5138403150698462}]}]},{"stamp_ns":1789379944613631236,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05401302894640339}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.2050725132752956,0.009904162403118507,0.16137315894988005,0.9653009248087268],"translation_xyz_m":[0.017600534826204068,0.05673110880143846,0.6634904296488253],"reprojection_error_px":0.23375960717230637},{"quaternion_xyzw":[0.12634435423060106,0.029328662247099915,0.15616598884766952,0.9791777763266881],"translation_xyz_m":[0.01758602853970864,0.05678431135902915,0.6634314207940983],"reprojection_error_px":0.5093052168337312}]}]},{"stamp_ns":1789379944646910441,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04854800278207007}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20495338175621997,0.009588163838527362,0.1613779356294567,0.9653286177840525],"translation_xyz_m":[0.017601666096045035,0.05673774577378649,0.663530115296939],"reprojection_error_px":0.23742677572373863},{"quaternion_xyzw":[0.12621822174366382,0.02964385751352319,0.15614729535931482,0.9791875327860113],"translation_xyz_m":[0.017587070450672707,0.056790936596769315,0.6634716743139799],"reprojection_error_px":0.5088189459980397}]}]},{"stamp_ns":1789379944680162589,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.05648454077854162}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20496950212961557,0.01000882856121646,0.1614137061895349,0.965314944462028],"translation_xyz_m":[0.0176007870116482,0.05673538385602693,0.6635421942120796],"reprojection_error_px":0.23575644492855294},{"quaternion_xyzw":[0.1262422382039402,0.02922637866357943,0.1562183744733426,0.979185649180164],"translation_xyz_m":[0.017586309698041876,0.0567885214340787,0.6634827757161623],"reprojection_error_px":0.5117674350017327}]}]},{"stamp_ns":1789379944713627135,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.04920428499260392}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20506634747636962,0.009942941157829401,0.16137699899169203,0.9653011940582423],"translation_xyz_m":[0.01759981222100435,0.05674218510928112,0.6635569476753783],"reprojection_error_px":0.24153047865349006},{"quaternion_xyzw":[0.12633018464982382,0.029285736297091875,0.1561745105231043,0.9791795301979854],"translation_xyz_m":[0.017585318234243377,0.05679536803521984,0.6634977905265985],"reprojection_error_px":0.513401023589377}]}]},{"stamp_ns":1789379944746811495,"direction":"decreasing","roles":[{"role":"top_base","frozen":true,"candidates":[{"quaternion_xyzw":[0.0012696792530900863,0.05075815238023551,-0.02257113431821763,0.9984550774957919],"translation_xyz_m":[-0.05542883300338809,-0.0030803753210777067,0.7372446342372366],"reprojection_error_px":0.06686259924957282}]},{"role":"thumb_yaw","frozen":false,"candidates":[{"quaternion_xyzw":[-0.20515063569226516,0.010044437740137574,0.16135311067101354,0.9652862267857817],"translation_xyz_m":[0.017598449158217434,0.05674312987683059,0.6635165672619672],"reprojection_error_px":0.23557961292691573},{"quaternion_xyzw":[0.12640805433420338,0.029184668225968984,0.15615682511990617,0.9791753187802745],"translation_xyz_m":[0.01758398722589903,0.056796346312069,0.6634574013564583],"reprojection_error_px":0.5106825939778069}]}]}]} diff --git a/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_branch_events.json b/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_branch_events.json new file mode 100644 index 0000000..d46cb1d --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o6_thumb_20260914_branch_events.json @@ -0,0 +1,3246 @@ +{ + "schema_version": 1, + "purpose": "Regression evidence for IPPE branch confirmation; no physical pose ground truth or accuracy certificate.", + "source": { + "relative_path": "calibration_output/O6_RIGHT_001/20260914_152745/raw_samples.jsonl", + "sha256": "dc4e4d8d641597446a09ea052c0c0a302daf49bc038e0da7720e41e6c0f4ad4e" + }, + "camera_matrix": [ + [ + 3792.732421875, + 0.0, + 606.0140166893543 + ], + [ + 0.0, + 3825.36962890625, + 568.5244182036549 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "tag_size_m": 0.016, + "zero_sequences": [ + { + "tag_id": 1, + "role": "front_id1", + "frames": [ + { + "stamp_ns": 1789370916861480078, + "corners_xy": [ + [ + 841.8316650390625, + 990.7797241210939 + ], + [ + 883.0867919921874, + 969.9808349609375 + ], + [ + 869.6893920898439, + 913.687744140625 + ], + [ + 828.9094848632812, + 934.0426025390624 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03607820950016547, + -0.3395053764254653, + -0.13800941507522935, + 0.9297246170388619 + ] + }, + { + "stamp_ns": 1789370916894457713, + "corners_xy": [ + [ + 841.4138793945312, + 990.8507690429688 + ], + [ + 882.7119140625001, + 970.0057373046876 + ], + [ + 869.2764892578125, + 913.7431640624999 + ], + [ + 828.503479003906, + 934.0870971679685 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03616127850746778, + -0.33933052800118957, + -0.1381368475263643, + 0.9297662964738059 + ] + }, + { + "stamp_ns": 1789370916927721494, + "corners_xy": [ + [ + 841.1685180664062, + 990.8760986328124 + ], + [ + 882.5025024414064, + 969.9891357421876 + ], + [ + 868.9929809570312, + 913.7767944335938 + ], + [ + 828.2429809570312, + 934.11083984375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03592074190303671, + -0.3390129392625997, + -0.1384522671668831, + 0.9298445553048199 + ] + }, + { + "stamp_ns": 1789370916961034466, + "corners_xy": [ + [ + 841.254333496094, + 990.8315429687499 + ], + [ + 882.5987548828125, + 969.9567260742188 + ], + [ + 869.0981445312499, + 913.7269897460939 + ], + [ + 828.3494873046875, + 934.05322265625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03595069744168794, + -0.3390911803780303, + -0.138312294082306, + 0.9298356994915715 + ] + }, + { + "stamp_ns": 1789370916994408138, + "corners_xy": [ + [ + 841.6057739257814, + 990.7489013671877 + ], + [ + 882.9135131835935, + 969.9093017578125 + ], + [ + 869.4608154296875, + 913.6425781249998 + ], + [ + 828.6882934570316, + 933.9756469726564 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03594130578553041, + -0.33927064779131244, + -0.13814470660418546, + 0.929795509842319 + ] + }, + { + "stamp_ns": 1789370917027596191, + "corners_xy": [ + [ + 841.8883056640625, + 990.723693847656 + ], + [ + 883.1617431640624, + 969.908203125 + ], + [ + 869.7276611328126, + 913.6284790039066 + ], + [ + 828.9460449218751, + 933.9785156250001 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.035863412720355356, + -0.33930996127281066, + -0.13816522486369617, + 0.9297811228711074 + ] + }, + { + "stamp_ns": 1789370917061064480, + "corners_xy": [ + [ + 841.8567504882811, + 990.7454223632814 + ], + [ + 883.1007080078124, + 969.9557495117188 + ], + [ + 869.7002563476564, + 913.6802978515624 + ], + [ + 828.9057617187501, + 934.0286254882812 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03588146070370756, + -0.3392823833211841, + -0.13809178132327854, + 0.9298014008790422 + ] + }, + { + "stamp_ns": 1789370917128352150, + "corners_xy": [ + [ + 841.1586303710936, + 990.8773193359374 + ], + [ + 882.5262451171875, + 969.9984741210938 + ], + [ + 869.0206909179688, + 913.7845458984375 + ], + [ + 828.2614746093749, + 934.1234130859374 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03615232285622747, + -0.33876744924847524, + -0.13838468692719458, + 0.9299351070401867 + ] + }, + { + "stamp_ns": 1789370917195100821, + "corners_xy": [ + [ + 841.5043945312499, + 990.7498168945311 + ], + [ + 882.83349609375, + 969.9080200195311 + ], + [ + 869.3903198242188, + 913.64990234375 + ], + [ + 828.616516113281, + 933.9862670898436 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.036243734754154926, + -0.3391289571388681, + -0.13809190975069116, + 0.9298433021656828 + ] + }, + { + "stamp_ns": 1789370917227807549, + "corners_xy": [ + [ + 841.8787231445314, + 990.7273559570314 + ], + [ + 883.190673828125, + 969.8950805664062 + ], + [ + 869.7398681640626, + 913.6215209960938 + ], + [ + 828.9703979492188, + 933.9711303710938 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.03607126272555533, + -0.33921339655208743, + -0.1381557787371635, + 0.9298097205377605 + ] + } + ] + }, + { + "tag_id": 2, + "role": "front_id2", + "frames": [ + { + "stamp_ns": 1789370916861480078, + "corners_xy": [ + [ + 977.9118041992188, + 937.711669921875 + ], + [ + 1024.3421630859377, + 916.9291992187501 + ], + [ + 999.6987915039061, + 859.61279296875 + ], + [ + 953.070861816406, + 880.763671875 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.001266320351692838, + 0.32656872846093815, + -0.21150393562344005, + 0.9212042918050093 + ] + }, + { + "stamp_ns": 1789370916894457713, + "corners_xy": [ + [ + 977.5369262695312, + 937.7326049804685 + ], + [ + 1023.9547119140625, + 916.97607421875 + ], + [ + 999.3069458007812, + 859.6720581054689 + ], + [ + 952.6351928710938, + 880.8204345703125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0005258636259036331, + 0.3261086895559081, + -0.21162673889503233, + 0.9213397687325937 + ] + }, + { + "stamp_ns": 1789370916927721494, + "corners_xy": [ + [ + 977.2832031249999, + 937.7637939453125 + ], + [ + 1023.720458984375, + 916.9885864257812 + ], + [ + 999.05126953125, + 859.7035522460939 + ], + [ + 952.4124755859373, + 880.8458251953125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0006664920238971564, + 0.32607087609411856, + -0.21164816771045836, + 0.9213481386839623 + ] + }, + { + "stamp_ns": 1789370916961034466, + "corners_xy": [ + [ + 977.3972778320312, + 937.7131347656251 + ], + [ + 1023.8067626953124, + 916.947021484375 + ], + [ + 999.1549682617188, + 859.6539306640625 + ], + [ + 952.5059204101564, + 880.8078002929689 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0006733255035164308, + 0.32617473827498955, + -0.21167436723796323, + 0.9213053505751587 + ] + }, + { + "stamp_ns": 1789370916994408138, + "corners_xy": [ + [ + 977.7373657226564, + 937.6246337890628 + ], + [ + 1024.1564941406248, + 916.8706054687501 + ], + [ + 999.4950561523439, + 859.5783081054686 + ], + [ + 952.850280761719, + 880.7221679687501 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.00048169563559425813, + 0.32619771051823543, + -0.21164797211436356, + 0.9213034014491935 + ] + }, + { + "stamp_ns": 1789370917027596191, + "corners_xy": [ + [ + 977.9763793945311, + 937.6466674804688 + ], + [ + 1024.409912109375, + 916.84521484375 + ], + [ + 999.7642211914062, + 859.5664062499998 + ], + [ + 953.1428833007814, + 880.7083740234374 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.00131244842363921, + 0.32632092714691024, + -0.21155647283227094, + 0.9212799730743554 + ] + }, + { + "stamp_ns": 1789370917061064480, + "corners_xy": [ + [ + 977.9276123046874, + 937.6957397460936 + ], + [ + 1024.3581542968748, + 916.8934936523436 + ], + [ + 999.7265625, + 859.603515625 + ], + [ + 953.1113891601564, + 880.7478027343749 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.001582503364223833, + 0.32647020988388153, + -0.21148063108211296, + 0.9212440721212743 + ] + }, + { + "stamp_ns": 1789370917128352150, + "corners_xy": [ + [ + 977.28125, + 937.7964477539064 + ], + [ + 1023.71875, + 916.9955444335939 + ], + [ + 999.0733032226562, + 859.7153320312499 + ], + [ + 952.44921875, + 880.8554077148438 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0012889908287015523, + 0.3262061428380159, + -0.21154367247443023, + 0.9213235943540241 + ] + }, + { + "stamp_ns": 1789370917195100821, + "corners_xy": [ + [ + 977.6334838867186, + 937.6550903320314 + ], + [ + 1024.0718994140627, + 916.8665161132812 + ], + [ + 999.40869140625, + 859.6055908203124 + ], + [ + 952.7425537109374, + 880.7398681640627 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0005498178438038254, + 0.32578056867264094, + -0.21169297210852359, + 0.9214406135696045 + ] + }, + { + "stamp_ns": 1789370917227807549, + "corners_xy": [ + [ + 977.9753417968751, + 937.6434936523436 + ], + [ + 1024.4312744140625, + 916.8594360351562 + ], + [ + 999.76611328125, + 859.56591796875 + ], + [ + 953.1282958984375, + 880.710754394531 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.0009603637309168912, + 0.32615018032900245, + -0.21156956177070868, + 0.921337863166935 + ] + } + ] + } + ], + "jump_events": [ + { + "tag_id": 1, + "role": "front_id1", + "direction": "decreasing", + "event_stamp_ns": 1789370922296085905, + "recorded_jump_deg": 91.55910878158785, + "recorded_gap_ms": 65.919522, + "warmup_frames": [ + { + "stamp_ns": 1789370921095364483, + "corners_xy": [ + [ + 839.8258056640624, + 977.4627685546876 + ], + [ + 875.5061645507814, + 942.9923095703125 + ], + [ + 852.6894531250001, + 893.9525146484376 + ], + [ + 817.3541259765625, + 927.384704589844 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.07997161350995657, + -0.3436948259218353, + -0.25360871710472294, + 0.9006447836276352 + ] + }, + { + "stamp_ns": 1789370921129797289, + "corners_xy": [ + [ + 839.8984375, + 977.1511840820311 + ], + [ + 875.4153442382814, + 942.3190307617188 + ], + [ + 852.3778686523438, + 893.5751953125 + ], + [ + 817.201904296875, + 927.242431640625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.08097966216118763, + -0.34345751114426837, + -0.25649905832606346, + 0.8998263529329907 + ] + }, + { + "stamp_ns": 1789370921162459173, + "corners_xy": [ + [ + 839.4080200195312, + 976.9478149414061 + ], + [ + 874.7769775390626, + 941.7701416015624 + ], + [ + 851.4960937499999, + 893.2681274414064 + ], + [ + 816.5066528320309, + 927.2470703124999 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.0822151895470835, + -0.34347662707552734, + -0.25944940910340164, + 0.8988606529248301 + ] + }, + { + "stamp_ns": 1789370921195909027, + "corners_xy": [ + [ + 838.8689575195311, + 976.5872192382815 + ], + [ + 874.0408325195311, + 941.24267578125 + ], + [ + 850.6489868164062, + 892.8413085937499 + ], + [ + 815.7800903320312, + 927.1160888671876 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.08308593490506988, + -0.3439411682814998, + -0.2615459189298744, + 0.8979949512515286 + ] + }, + { + "stamp_ns": 1789370921229096404, + "corners_xy": [ + [ + 838.7727661132811, + 976.2608642578122 + ], + [ + 873.7934570312499, + 940.6082153320311 + ], + [ + 850.1907348632812, + 892.458923339844 + ], + [ + 815.4801025390625, + 927.0161743164062 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.08428710921314078, + -0.343651418890193, + -0.2643702571122492, + 0.8971665133461304 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370922028948426, + "corners_xy": [ + [ + 836.6953124999999, + 970.0242919921874 + ], + [ + 867.8793945312499, + 928.3171997070312 + ], + [ + 840.2654418945314, + 885.2573852539062 + ], + [ + 809.2293701171876, + 925.439453125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.10580678753892993, + -0.3433813453220876, + -0.321029483952698, + 0.8762615168022325 + ] + }, + { + "stamp_ns": 1789370922063153361, + "corners_xy": [ + [ + 836.6940307617188, + 969.7000122070312 + ], + [ + 867.7054443359376, + 927.8539428710939 + ], + [ + 839.9741210937501, + 884.9164428710939 + ], + [ + 809.0816650390625, + 925.3494262695314 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.1065090646521196, + -0.3435929576276985, + -0.3229812816387691, + 0.8753757994864602 + ] + }, + { + "stamp_ns": 1789370922096247132, + "corners_xy": [ + [ + 837.0845947265625, + 969.4166259765624 + ], + [ + 867.9137573242186, + 927.351318359375 + ], + [ + 840.0367431640626, + 884.574890136719 + ], + [ + 809.320983886719, + 925.2504882812501 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.10723091157361608, + -0.3440216999974183, + -0.3250998664850035, + 0.8743344201993939 + ] + }, + { + "stamp_ns": 1789370922162320566, + "corners_xy": [ + [ + 837.1031494140625, + 968.8704833984376 + ], + [ + 867.6008911132812, + 926.4024658203125 + ], + [ + 839.3906860351562, + 884.0574951171875 + ], + [ + 808.9993286132812, + 925.1925048828125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.10871966258770735, + -0.3435342510133844, + -0.32984884777617746, + 0.8725617404907323 + ] + }, + { + "stamp_ns": 1789370922230166383, + "corners_xy": [ + [ + 836.0722045898438, + 968.4658813476564 + ], + [ + 866.2341918945311, + 925.5650634765625 + ], + [ + 837.767822265625, + 883.6444091796875 + ], + [ + 807.6866455078125, + 925.1683959960939 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.11044681467294877, + -0.3437560178848806, + -0.3339953085658799, + 0.8706781467066413 + ] + }, + { + "stamp_ns": 1789370922296085905, + "corners_xy": [ + [ + 836.3759765625, + 967.9154663085938 + ], + [ + 866.2333984375, + 924.3694458007812 + ], + [ + 837.3726806640625, + 883.0837402343751 + ], + [ + 807.6739501953125, + 925.0364379882815 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.2131671567105631, + 0.3620375679974139, + -0.3621962213103174, + 0.8320471500624107 + ] + }, + { + "stamp_ns": 1789370922330085066, + "corners_xy": [ + [ + 836.6619873046875, + 967.6276855468749 + ], + [ + 866.311767578125, + 923.9971313476564 + ], + [ + 837.3663330078124, + 882.7891235351564 + ], + [ + 807.8192749023438, + 924.9837036132811 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.21352020125449206, + 0.3624233738594788, + -0.36391522746736865, + 0.8310379828587422 + ] + }, + { + "stamp_ns": 1789370922362463274, + "corners_xy": [ + [ + 836.5229492187501, + 967.4188232421875 + ], + [ + 866.0012817382811, + 923.6926269531252 + ], + [ + 836.9627685546872, + 882.6209716796875 + ], + [ + 807.5579833984375, + 924.9910888671873 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.213876097423752, + 0.3623306286636405, + -0.3655849139234229, + 0.8302536968870421 + ] + }, + { + "stamp_ns": 1789370922395991839, + "corners_xy": [ + [ + 835.9959716796875, + 967.2696533203125 + ], + [ + 865.3234863281249, + 923.2683105468751 + ], + [ + 836.1512451171875, + 882.4584350585939 + ], + [ + 806.8876342773439, + 925.01611328125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.2150017435986271, + 0.36179324511465877, + -0.3676841467453175, + 0.8292697186507673 + ] + }, + { + "stamp_ns": 1789370922429246027, + "corners_xy": [ + [ + 835.4738159179689, + 967.0714721679689 + ], + [ + 864.6260375976564, + 922.8685913085936 + ], + [ + 835.32763671875, + 882.2757568359376 + ], + [ + 806.2278442382814, + 925.0168457031253 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.21569246771170042, + 0.3615196245368799, + -0.36970576668729455, + 0.8283103081124309 + ] + }, + { + "stamp_ns": 1789370922463554332, + "corners_xy": [ + [ + 835.4122314453124, + 966.9015502929688 + ], + [ + 864.4597778320312, + 922.6525878906251 + ], + [ + 835.1014404296878, + 882.1359863281251 + ], + [ + 806.0988769531251, + 924.9810791015623 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.2158204770218892, + 0.3615281042735249, + -0.37070458936611433, + 0.82782670827978 + ] + } + ] + }, + { + "tag_id": 1, + "role": "front_id1", + "direction": "increasing", + "event_stamp_ns": 1789370930097956078, + "recorded_jump_deg": 90.18490963287228, + "recorded_gap_ms": 65.625308, + "warmup_frames": [ + { + "stamp_ns": 1789370928630917571, + "corners_xy": [ + [ + 835.7828979492186, + 965.6473388671874 + ], + [ + 864.0387573242188, + 920.3460083007814 + ], + [ + 833.9988403320314, + 881.0 + ], + [ + 805.7769775390625, + 924.7963256835935 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.22020784944226518, + 0.35919974152710016, + -0.3810012863151606, + 0.8229957889060369 + ] + }, + { + "stamp_ns": 1789370928664346480, + "corners_xy": [ + [ + 835.1138916015624, + 965.8469848632811 + ], + [ + 863.4817504882812, + 920.76123046875 + ], + [ + 833.5530395507815, + 881.1720581054688 + ], + [ + 805.2257080078126, + 924.8674926757814 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.2194207359812469, + 0.35976355457585224, + -0.3793771418509754, + 0.8237097241512817 + ] + }, + { + "stamp_ns": 1789370928697713144, + "corners_xy": [ + [ + 834.7032470703125, + 966.073486328125 + ], + [ + 863.2654418945312, + 921.0816650390626 + ], + [ + 833.4440917968751, + 881.3640747070311 + ], + [ + 804.9645385742189, + 924.8258666992186 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.21895233079922402, + 0.35985601849672516, + -0.3774733064267756, + 0.8246680700284867 + ] + }, + { + "stamp_ns": 1789370928731052870, + "corners_xy": [ + [ + 835.2678222656252, + 966.1373291015625 + ], + [ + 863.9345703125, + 921.3554077148436 + ], + [ + 834.2341918945309, + 881.4342651367186 + ], + [ + 805.6052856445311, + 924.7747802734376 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.21824767648068116, + 0.36008077617061307, + -0.3759079871816452, + 0.8254713632320608 + ] + }, + { + "stamp_ns": 1789370928764382345, + "corners_xy": [ + [ + 836.303466796875, + 966.3197631835939 + ], + [ + 865.0911254882812, + 921.7019653320312 + ], + [ + 835.5068359375001, + 881.6124267578124 + ], + [ + 806.7562255859375, + 924.8033447265625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.2177740613604027, + 0.36051009105188553, + -0.37428197824192766, + 0.8261476461333446 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370929864700721, + "corners_xy": [ + [ + 838.3619995117189, + 974.6395263671875 + ], + [ + 872.50048828125, + 937.2962036132812 + ], + [ + 847.8056030273438, + 890.4665527343751 + ], + [ + 813.9796142578125, + 926.442626953125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.19094928906392084, + 0.37025716483817567, + -0.30401907820542573, + 0.8567499057362669 + ] + }, + { + "stamp_ns": 1789370929931070236, + "corners_xy": [ + [ + 838.7575683593751, + 975.165954589844 + ], + [ + 873.181884765625, + 938.4609375 + ], + [ + 848.877685546875, + 891.1187744140624 + ], + [ + 814.7384033203125, + 926.6521606445312 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18897343996733787, + 0.3711505885087869, + -0.2991604763595604, + 0.8585099236589017 + ] + }, + { + "stamp_ns": 1789370929964759209, + "corners_xy": [ + [ + 839.3789672851562, + 975.3944702148438 + ], + [ + 873.9630126953126, + 939.06640625 + ], + [ + 849.8925781249999, + 891.3961791992186 + ], + [ + 815.605529785156, + 926.6990356445312 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18798561730384117, + 0.3721321495124909, + -0.29625885392483053, + 0.8593077227962361 + ] + }, + { + "stamp_ns": 1789370929998372917, + "corners_xy": [ + [ + 839.6222534179689, + 975.6032104492189 + ], + [ + 874.3583374023435, + 939.4280395507814 + ], + [ + 850.4046020507812, + 891.6852416992186 + ], + [ + 815.983093261719, + 926.6939086914061 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18705140345145163, + 0.3719429873365054, + -0.2944180447405888, + 0.860225668978302 + ] + }, + { + "stamp_ns": 1789370930032330770, + "corners_xy": [ + [ + 839.2894897460936, + 975.9013061523436 + ], + [ + 874.1888427734376, + 940.04638671875 + ], + [ + 850.43359375, + 892.01513671875 + ], + [ + 815.8486328124999, + 926.820556640625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18615717643022286, + 0.3724131959119609, + -0.2918842670272198, + 0.86107925990361 + ] + }, + { + "stamp_ns": 1789370930097956078, + "corners_xy": [ + [ + 838.8333740234375, + 976.5901489257815 + ], + [ + 874.0391845703124, + 941.34228515625 + ], + [ + 850.7339477539061, + 892.8675537109372 + ], + [ + 815.810791015625, + 927.1043701171874 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.08320545558543778, + -0.343946110695555, + -0.2608080408390352, + 0.898196576998557 + ] + }, + { + "stamp_ns": 1789370930131457637, + "corners_xy": [ + [ + 839.3335571289061, + 976.9655761718752 + ], + [ + 874.7379150390624, + 941.8244018554688 + ], + [ + 851.5213623046874, + 893.3334350585935 + ], + [ + 816.4851684570315, + 927.2212524414064 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.08228726823762594, + -0.34323221509429763, + -0.2589053558550265, + 0.8991042590918101 + ] + }, + { + "stamp_ns": 1789370930198294480, + "corners_xy": [ + [ + 839.9763793945312, + 977.4783935546874 + ], + [ + 875.6717529296874, + 943.1837768554686 + ], + [ + 852.982177734375, + 894.0585327148438 + ], + [ + 817.5853271484376, + 927.3634033203126 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.07946133758657888, + -0.34378184911448845, + -0.2524263065386496, + 0.9009888433358265 + ] + }, + { + "stamp_ns": 1789370930231325884, + "corners_xy": [ + [ + 839.6778564453126, + 977.7932128906249 + ], + [ + 875.5729370117188, + 943.6491088867188 + ], + [ + 853.0252075195312, + 894.4085083007812 + ], + [ + 817.5018920898439, + 927.4254760742186 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.07896182780283155, + -0.3436266285768117, + -0.2501599134681158, + 0.9017237867421457 + ] + }, + { + "stamp_ns": 1789370930298301036, + "corners_xy": [ + [ + 839.33203125, + 978.4237060546875 + ], + [ + 875.50244140625, + 945.019775390625 + ], + [ + 853.4163818359375, + 895.2311401367188 + ], + [ + 817.5362548828125, + 927.7412109375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.07654160782491533, + -0.3437668281699731, + -0.24451581292014224, + 0.9034255737765756 + ] + }, + { + "stamp_ns": 1789370930331234471, + "corners_xy": [ + [ + 839.8162231445312, + 978.8532104492188 + ], + [ + 876.193115234375, + 945.6693115234375 + ], + [ + 854.2601318359376, + 895.7555541992188 + ], + [ + 818.2580566406251, + 927.9335327148439 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.07565243556892182, + -0.3434210691755752, + -0.24200481568048102, + 0.9043076619305241 + ] + } + ] + }, + { + "tag_id": 2, + "role": "front_id2", + "direction": "decreasing", + "event_stamp_ns": 1789370920061955074, + "recorded_jump_deg": 60.69083719300058, + "recorded_gap_ms": 99.982047, + "warmup_frames": [ + { + "stamp_ns": 1789370919361570729, + "corners_xy": [ + [ + 976.69189453125, + 928.8937988281248 + ], + [ + 1021.8784790039061, + 904.1541137695311 + ], + [ + 994.2962646484374, + 848.5629882812501 + ], + [ + 948.9361572265625, + 873.5911254882811 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.013410137057213022, + 0.31741558731841235, + -0.24139673233657405, + 0.9169488157845227 + ] + }, + { + "stamp_ns": 1789370919396116606, + "corners_xy": [ + [ + 976.5182495117189, + 927.9889526367188 + ], + [ + 1021.5270996093748, + 902.8389892578124 + ], + [ + 993.6715698242188, + 847.4262695312499 + ], + [ + 948.4406738281252, + 872.8683471679688 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.014908557357918588, + 0.3166755368875473, + -0.2445182296446125, + 0.9163542844478624 + ] + }, + { + "stamp_ns": 1789370919429450579, + "corners_xy": [ + [ + 976.6777343750001, + 926.8379516601564 + ], + [ + 1021.524353027344, + 901.2255859375 + ], + [ + 993.294921875, + 846.0426635742188 + ], + [ + 948.2687988281249, + 871.941162109375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01626729249163727, + 0.315775514940663, + -0.2481551404959396, + 0.9156638169133862 + ] + }, + { + "stamp_ns": 1789370919462760466, + "corners_xy": [ + [ + 976.9521484375, + 925.9243774414064 + ], + [ + 1021.6847534179686, + 899.9271240234375 + ], + [ + 993.1724853515625, + 844.955383300781 + ], + [ + 948.2875366210938, + 871.2058715820311 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01751623705515644, + 0.3146337464405695, + -0.2510157140122548, + 0.9152540075624453 + ] + }, + { + "stamp_ns": 1789370919495118023, + "corners_xy": [ + [ + 976.9638061523435, + 924.8317260742186 + ], + [ + 1021.5001831054689, + 898.3319702148438 + ], + [ + 992.5918579101562, + 843.5900268554689 + ], + [ + 947.9219360351561, + 870.3668823242189 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.019212654030651706, + 0.3137275376930207, + -0.2550232192953354, + 0.9144228035424401 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370919761851239, + "corners_xy": [ + [ + 975.2710571289061, + 916.734680175781 + ], + [ + 1018.4137573242186, + 886.74609375 + ], + [ + 986.9385986328125, + 833.9595947265626 + ], + [ + 943.7398071289062, + 864.117919921875 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.031162836473013503, + 0.30463457594714627, + -0.2822062026032893, + 0.9091679228683168 + ] + }, + { + "stamp_ns": 1789370919795670012, + "corners_xy": [ + [ + 974.9169921874998, + 915.6630249023439 + ], + [ + 1017.8367919921874, + 885.2283935546875 + ], + [ + 986.0452270507812, + 832.6918334960938 + ], + [ + 943.0424804687499, + 863.299072265625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.03277263608693133, + 0.303700447429458, + -0.28577380977556877, + 0.9083090455353935 + ] + }, + { + "stamp_ns": 1789370919862014852, + "corners_xy": [ + [ + 974.9357299804686, + 912.6576538085938 + ], + [ + 1017.3471069335938, + 881.1002807617189 + ], + [ + 984.7008666992189, + 829.2613525390626 + ], + [ + 942.2231445312501, + 861.0001220703125 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.036605825126248846, + 0.3004079468759505, + -0.2949641932831673, + 0.9063173857437653 + ] + }, + { + "stamp_ns": 1789370919928422769, + "corners_xy": [ + [ + 974.6786499023436, + 910.3790893554688 + ], + [ + 1016.6150512695312, + 877.8235473632812 + ], + [ + 983.259765625, + 826.63818359375 + ], + [ + 941.2305908203124, + 859.3309936523436 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.04018279653434243, + 0.2973763424877821, + -0.30295445508338803, + 0.9045281930025157 + ] + }, + { + "stamp_ns": 1789370919961973027, + "corners_xy": [ + [ + 973.8916625976561, + 908.9640502929688 + ], + [ + 1015.5842285156252, + 875.8530883789062 + ], + [ + 981.7790527343749, + 825.049072265625 + ], + [ + 940.0612182617185, + 858.3026123046876 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.042153725643048895, + 0.29544945648097104, + -0.30761273693990576, + 0.9034971423041966 + ] + }, + { + "stamp_ns": 1789370920061955074, + "corners_xy": [ + [ + 973.8348999023435, + 906.1541137695311 + ], + [ + 1014.8951416015625, + 871.9894409179688 + ], + [ + 980.3778076171875, + 821.9412841796876 + ], + [ + 939.1864013671875, + 856.203308105469 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.05416882685936367, + -0.22724886577359987, + -0.29594445539375935, + 0.926196831414891 + ] + }, + { + "stamp_ns": 1789370920095154477, + "corners_xy": [ + [ + 973.8138427734376, + 905.3861083984375 + ], + [ + 1014.7245483398439, + 870.8510131835938 + ], + [ + 979.9180297851561, + 821.054931640625 + ], + [ + 938.9485473632812, + 855.6458740234376 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.05273772442385162, + -0.22668890810860018, + -0.2990622335024095, + 0.9254148539199458 + ] + }, + { + "stamp_ns": 1789370920128974762, + "corners_xy": [ + [ + 973.3208618164061, + 903.7847900390624 + ], + [ + 1013.8903808593749, + 868.5779418945312 + ], + [ + 978.5587158203125, + 819.278076171875 + ], + [ + 937.9981689453126, + 854.5272827148438 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.05015266647589326, + -0.22533185025559413, + -0.30522648556140597, + 0.923876106313818 + ] + }, + { + "stamp_ns": 1789370920161863354, + "corners_xy": [ + [ + 972.4733886718751, + 901.664306640625 + ], + [ + 1012.6113281250001, + 865.6201171875001 + ], + [ + 976.7182006835939, + 816.9604492187501 + ], + [ + 936.5767211914061, + 852.9603881835936 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.04667638314994573, + -0.22319875413110643, + -0.31257370280727864, + 0.9221178404757758 + ] + }, + { + "stamp_ns": 1789370920196021770, + "corners_xy": [ + [ + 971.8222045898439, + 899.9313964843752 + ], + [ + 1011.5064697265623, + 863.1991577148438 + ], + [ + 975.1532592773438, + 814.9866943359374 + ], + [ + 935.4021606445316, + 851.7625732421876 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.04293653740190001, + -0.2225607468484815, + -0.31899949925281285, + 0.920251317409889 + ] + }, + { + "stamp_ns": 1789370920229050863, + "corners_xy": [ + [ + 971.6786499023438, + 898.7551879882814 + ], + [ + 1011.1019287109376, + 861.4962768554689 + ], + [ + 974.322998046875, + 813.7354736328126 + ], + [ + 934.8778076171875, + 850.9512329101564 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.04170649630066047, + -0.22092173792377884, + -0.3237952609279164, + 0.9190270849543459 + ] + } + ] + }, + { + "tag_id": 2, + "role": "front_id2", + "direction": "decreasing", + "event_stamp_ns": 1789370920962003913, + "recorded_jump_deg": 52.172779141203115, + "recorded_gap_ms": 33.398333, + "warmup_frames": [ + { + "stamp_ns": 1789370919361570729, + "corners_xy": [ + [ + 976.69189453125, + 928.8937988281248 + ], + [ + 1021.8784790039061, + 904.1541137695311 + ], + [ + 994.2962646484374, + 848.5629882812501 + ], + [ + 948.9361572265625, + 873.5911254882811 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.013410137057213022, + 0.31741558731841235, + -0.24139673233657405, + 0.9169488157845227 + ] + }, + { + "stamp_ns": 1789370919396116606, + "corners_xy": [ + [ + 976.5182495117189, + 927.9889526367188 + ], + [ + 1021.5270996093748, + 902.8389892578124 + ], + [ + 993.6715698242188, + 847.4262695312499 + ], + [ + 948.4406738281252, + 872.8683471679688 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.014908557357918588, + 0.3166755368875473, + -0.2445182296446125, + 0.9163542844478624 + ] + }, + { + "stamp_ns": 1789370919429450579, + "corners_xy": [ + [ + 976.6777343750001, + 926.8379516601564 + ], + [ + 1021.524353027344, + 901.2255859375 + ], + [ + 993.294921875, + 846.0426635742188 + ], + [ + 948.2687988281249, + 871.941162109375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01626729249163727, + 0.315775514940663, + -0.2481551404959396, + 0.9156638169133862 + ] + }, + { + "stamp_ns": 1789370919462760466, + "corners_xy": [ + [ + 976.9521484375, + 925.9243774414064 + ], + [ + 1021.6847534179686, + 899.9271240234375 + ], + [ + 993.1724853515625, + 844.955383300781 + ], + [ + 948.2875366210938, + 871.2058715820311 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01751623705515644, + 0.3146337464405695, + -0.2510157140122548, + 0.9152540075624453 + ] + }, + { + "stamp_ns": 1789370919495118023, + "corners_xy": [ + [ + 976.9638061523435, + 924.8317260742186 + ], + [ + 1021.5001831054689, + 898.3319702148438 + ], + [ + 992.5918579101562, + 843.5900268554689 + ], + [ + 947.9219360351561, + 870.3668823242189 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.019212654030651706, + 0.3137275376930207, + -0.2550232192953354, + 0.9144228035424401 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370920795509073, + "corners_xy": [ + [ + 963.1979370117188, + 870.7359008789062 + ], + [ + 995.3173217773439, + 823.6604003906249 + ], + [ + 951.6177978515625, + 785.6359252929688 + ], + [ + 919.5165405273438, + 832.4290161132814 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.009566308396531161, + -0.19571894740367798, + -0.4227370348289301, + 0.8848140927646342 + ] + }, + { + "stamp_ns": 1789370920828599436, + "corners_xy": [ + [ + 962.5545654296875, + 869.3079833984376 + ], + [ + 994.1810913085938, + 821.6923828124999 + ], + [ + 950.1202392578124, + 784.3186645507812 + ], + [ + 918.5248413085935, + 831.6180419921876 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.01302197962303367, + -0.19457422359602636, + -0.4284820681099176, + 0.8822553014105093 + ] + }, + { + "stamp_ns": 1789370920862076322, + "corners_xy": [ + [ + 962.1535034179689, + 867.4873657226562 + ], + [ + 993.2343139648436, + 819.3088378906251 + ], + [ + 948.7570190429685, + 782.6868896484375 + ], + [ + 917.7096557617189, + 830.509521484375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.016078152855180272, + -0.19244693795337128, + -0.4349624889855017, + 0.8794960501609284 + ] + }, + { + "stamp_ns": 1789370920895506643, + "corners_xy": [ + [ + 961.5960083007814, + 865.2955322265626 + ], + [ + 992.0611572265625, + 816.4982299804686 + ], + [ + 947.1793823242189, + 780.6943969726561 + ], + [ + 916.7544555664064, + 829.1378784179688 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.020875840669794848, + -0.19062343234950746, + -0.4420360824935144, + 0.8762596693271972 + ] + }, + { + "stamp_ns": 1789370920928605580, + "corners_xy": [ + [ + 960.8394775390625, + 863.5663452148438 + ], + [ + 990.7080078125, + 814.1691284179688 + ], + [ + 945.4577636718749, + 779.1938476562498 + ], + [ + 915.5734252929688, + 828.1383666992186 + ] + ], + "recorded_selected_quaternion_xyzw": [ + 0.0246665741853686, + -0.18833752018173014, + -0.4486745268746124, + 0.8732764210395221 + ] + }, + { + "stamp_ns": 1789370920962003913, + "corners_xy": [ + [ + 959.8809814453125, + 861.8621826171875 + ], + [ + 989.1842041015625, + 811.8917236328125 + ], + [ + 943.488525390625, + 777.6652221679688 + ], + [ + 914.3042602539062, + 827.1735839843751 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.1262924512706716, + 0.23593109178131416, + -0.461941737462207, + 0.8455746968029019 + ] + }, + { + "stamp_ns": 1789370920995289791, + "corners_xy": [ + [ + 958.7778930664062, + 859.8155517578125 + ], + [ + 987.4425048828125, + 809.2586669921874 + ], + [ + 941.3956909179688, + 775.9148559570312 + ], + [ + 912.7879638671877, + 825.9402465820315 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.1304651146065109, + 0.2325785497881161, + -0.4682120251060998, + 0.8424153201332534 + ] + }, + { + "stamp_ns": 1789370921029055821, + "corners_xy": [ + [ + 957.691162109375, + 857.8050537109375 + ], + [ + 985.7128295898438, + 806.7048339843751 + ], + [ + 939.2355346679688, + 774.2397460937501 + ], + [ + 911.303955078125, + 824.8272094726564 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.1341351880041114, + 0.22927869735866427, + -0.47484773316706, + 0.8390224434320696 + ] + }, + { + "stamp_ns": 1789370921062055109, + "corners_xy": [ + [ + 957.0828857421877, + 856.0609130859375 + ], + [ + 984.4624633789061, + 804.4253540039062 + ], + [ + 937.6751098632811, + 772.7856445312501 + ], + [ + 910.369873046875, + 823.8724975585938 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.13852752730863468, + 0.22744518685773601, + -0.480696882802667, + 0.8354695195019408 + ] + }, + { + "stamp_ns": 1789370921095364483, + "corners_xy": [ + [ + 956.64599609375, + 853.9356079101562 + ], + [ + 983.3109741210936, + 801.7766113281248 + ], + [ + 936.1190185546874, + 771.0383300781249 + ], + [ + 909.4986572265625, + 822.665710449219 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.14177501179699767, + 0.22445373092018311, + -0.4874455341490432, + 0.8318156165546096 + ] + }, + { + "stamp_ns": 1789370921129797289, + "corners_xy": [ + [ + 955.8729248046876, + 852.0794067382812 + ], + [ + 981.8500366210938, + 799.4000244140624 + ], + [ + 934.3607788085935, + 769.5193481445312 + ], + [ + 908.4054565429686, + 821.631591796875 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.14602252759380216, + 0.22288254627544385, + -0.4934132422745433, + 0.8279759443057598 + ] + } + ] + }, + { + "tag_id": 2, + "role": "front_id2", + "direction": "increasing", + "event_stamp_ns": 1789370930665419865, + "recorded_jump_deg": 54.32884152446253, + "recorded_gap_ms": 100.683568, + "warmup_frames": [ + { + "stamp_ns": 1789370929731035320, + "corners_xy": [ + [ + 943.0989379882812, + 830.541259765625 + ], + [ + 960.5886840820312, + 772.9827270507812 + ], + [ + 909.936767578125, + 754.0853271484376 + ], + [ + 892.3124389648438, + 810.9876708984376 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.19206320171589322, + 0.19476164198264273, + -0.5646778586231859, + 0.7786645910400692 + ] + }, + { + "stamp_ns": 1789370929764646493, + "corners_xy": [ + [ + 944.6932373046876, + 832.0878295898438 + ], + [ + 962.9051513671875, + 774.8395996093752 + ], + [ + 912.4093017578123, + 755.07666015625 + ], + [ + 894.1331787109375, + 811.6642456054686 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18868312163403766, + 0.19642553394876486, + -0.5593451929567459, + 0.7829103680112802 + ] + }, + { + "stamp_ns": 1789370929797929367, + "corners_xy": [ + [ + 945.954833984375, + 833.7257080078124 + ], + [ + 964.8338623046875, + 776.7712402343751 + ], + [ + 914.5651245117189, + 756.1362304687501 + ], + [ + 895.5770263671875, + 812.3894042968749 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18517576434934724, + 0.1985183141056056, + -0.5538568601916564, + 0.7871105346016803 + ] + }, + { + "stamp_ns": 1789370929831339570, + "corners_xy": [ + [ + 946.6353759765624, + 835.408935546875 + ], + [ + 966.23974609375, + 778.8134155273436 + ], + [ + 916.1669311523438, + 757.2557373046874 + ], + [ + 896.4816284179686, + 813.1784057617189 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18133309834258848, + 0.20029682163780962, + -0.5482368021127334, + 0.7914770366198076 + ] + }, + { + "stamp_ns": 1789370929864700721, + "corners_xy": [ + [ + 947.0184936523435, + 837.17431640625 + ], + [ + 967.32666015625, + 780.9005737304689 + ], + [ + 917.4643554687502, + 758.4805908203125 + ], + [ + 897.1113891601561, + 814.0443115234373 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.17782236438258434, + 0.20229475045694326, + -0.5426826358257137, + 0.7955825522447096 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370930431407877, + "corners_xy": [ + [ + 962.2660522460938, + 867.7261352539062 + ], + [ + 993.4649047851562, + 819.688720703125 + ], + [ + 949.1262817382814, + 782.8763427734376 + ], + [ + 917.9469604492186, + 830.5552978515624 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.11384560881507469, + 0.2439358567189025, + -0.4418976409479291, + 0.8557224725838368 + ] + }, + { + "stamp_ns": 1789370930465740338, + "corners_xy": [ + [ + 962.5813598632812, + 869.5241699218751 + ], + [ + 994.3773193359375, + 822.0857543945314 + ], + [ + 950.413330078125, + 784.5078735351564 + ], + [ + 918.6743164062499, + 831.6265869140625 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.109990515000417, + 0.24617608922751844, + -0.43587051246743347, + 0.858671250283812 + ] + }, + { + "stamp_ns": 1789370930498088080, + "corners_xy": [ + [ + 963.3319702148438, + 871.4411010742188 + ], + [ + 995.649658203125, + 824.6180419921875 + ], + [ + 952.1329345703126, + 786.2709350585939 + ], + [ + 919.8014526367188, + 832.8319702148438 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.10636309383835026, + 0.24882778763265342, + -0.4298009914077793, + 0.8614190224019107 + ] + }, + { + "stamp_ns": 1789370930531893491, + "corners_xy": [ + [ + 964.4313354492186, + 873.4391479492188 + ], + [ + 997.3539428710938, + 827.2446289062501 + ], + [ + 954.2367553710939, + 788.1530151367189 + ], + [ + 921.36279296875, + 834.0894775390624 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.10253980675967, + 0.25127078153502885, + -0.4235433249364562, + 0.8642682652268546 + ] + }, + { + "stamp_ns": 1789370930564736297, + "corners_xy": [ + [ + 965.4403686523438, + 875.25390625 + ], + [ + 998.8563842773436, + 829.6345825195311 + ], + [ + 956.1741333007812, + 789.8638305664062 + ], + [ + 922.7854614257812, + 835.247802734375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.09972989166978842, + 0.2538541557718456, + -0.41776962510407945, + 0.8666490389109611 + ] + }, + { + "stamp_ns": 1789370930665419865, + "corners_xy": [ + [ + 966.4263916015624, + 880.3541870117186 + ], + [ + 1001.3024902343749, + 836.5289306640627 + ], + [ + 959.8629760742186, + 794.781005859375 + ], + [ + 924.9700317382812, + 838.451721191406 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.00953064800068983, + -0.2050364065665294, + -0.38820821636080927, + 0.8984228511569209 + ] + }, + { + "stamp_ns": 1789370930731729704, + "corners_xy": [ + [ + 968.0409545898438, + 884.1393432617186 + ], + [ + 1003.8702392578126, + 841.6478881835939 + ], + [ + 963.4245605468751, + 798.5460205078126 + ], + [ + 927.5225219726562, + 840.9422607421874 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.015983192433045235, + -0.20901832902961925, + -0.37472350023864154, + 0.9031268870198778 + ] + }, + { + "stamp_ns": 1789370930764713255, + "corners_xy": [ + [ + 968.8762207031249, + 885.7640380859373 + ], + [ + 1005.2083129882812, + 843.8292236328124 + ], + [ + 965.096435546875, + 800.2379760742189 + ], + [ + 928.8018188476561, + 842.0301513671875 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01929029811220657, + -0.20932071598447238, + -0.369162946611708, + 0.9052797584766001 + ] + }, + { + "stamp_ns": 1789370930798120613, + "corners_xy": [ + [ + 969.2769165039062, + 887.4396972656249 + ], + [ + 1006.0454711914062, + 846.0938110351562 + ], + [ + 966.3150634765625, + 801.9194946289065 + ], + [ + 929.6320190429688, + 843.1549682617189 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.02233169469114071, + -0.21115860852373763, + -0.363318387767148, + 0.9071455707706568 + ] + }, + { + "stamp_ns": 1789370930831713263, + "corners_xy": [ + [ + 969.1282958984374, + 888.5097045898436 + ], + [ + 1006.1492309570312, + 847.5663452148438 + ], + [ + 966.7294921874999, + 802.9868774414062 + ], + [ + 929.7163085937499, + 843.835693359375 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.024503763448868815, + -0.21243099627939832, + -0.35926991059022345, + 0.9084039678144723 + ] + }, + { + "stamp_ns": 1789370930864729471, + "corners_xy": [ + [ + 969.2744750976562, + 890.1657104492185 + ], + [ + 1006.7484741210939, + 849.8535766601562 + ], + [ + 967.7740478515625, + 804.7062377929689 + ], + [ + 930.3048095703125, + 844.9215698242188 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.02771696669635058, + -0.2136437351610206, + -0.3530783326196701, + 0.9104525332042932 + ] + } + ] + }, + { + "tag_id": 2, + "role": "front_id2", + "direction": "increasing", + "event_stamp_ns": 1789370931732324975, + "recorded_jump_deg": 64.86663489461752, + "recorded_gap_ms": 33.948157, + "warmup_frames": [ + { + "stamp_ns": 1789370929731035320, + "corners_xy": [ + [ + 943.0989379882812, + 830.541259765625 + ], + [ + 960.5886840820312, + 772.9827270507812 + ], + [ + 909.936767578125, + 754.0853271484376 + ], + [ + 892.3124389648438, + 810.9876708984376 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.19206320171589322, + 0.19476164198264273, + -0.5646778586231859, + 0.7786645910400692 + ] + }, + { + "stamp_ns": 1789370929764646493, + "corners_xy": [ + [ + 944.6932373046876, + 832.0878295898438 + ], + [ + 962.9051513671875, + 774.8395996093752 + ], + [ + 912.4093017578123, + 755.07666015625 + ], + [ + 894.1331787109375, + 811.6642456054686 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18868312163403766, + 0.19642553394876486, + -0.5593451929567459, + 0.7829103680112802 + ] + }, + { + "stamp_ns": 1789370929797929367, + "corners_xy": [ + [ + 945.954833984375, + 833.7257080078124 + ], + [ + 964.8338623046875, + 776.7712402343751 + ], + [ + 914.5651245117189, + 756.1362304687501 + ], + [ + 895.5770263671875, + 812.3894042968749 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18517576434934724, + 0.1985183141056056, + -0.5538568601916564, + 0.7871105346016803 + ] + }, + { + "stamp_ns": 1789370929831339570, + "corners_xy": [ + [ + 946.6353759765624, + 835.408935546875 + ], + [ + 966.23974609375, + 778.8134155273436 + ], + [ + 916.1669311523438, + 757.2557373046874 + ], + [ + 896.4816284179686, + 813.1784057617189 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.18133309834258848, + 0.20029682163780962, + -0.5482368021127334, + 0.7914770366198076 + ] + }, + { + "stamp_ns": 1789370929864700721, + "corners_xy": [ + [ + 947.0184936523435, + 837.17431640625 + ], + [ + 967.32666015625, + 780.9005737304689 + ], + [ + 917.4643554687502, + 758.4805908203125 + ], + [ + 897.1113891601561, + 814.0443115234373 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.17782236438258434, + 0.20229475045694326, + -0.5426826358257137, + 0.7955825522447096 + ] + } + ], + "frames": [ + { + "stamp_ns": 1789370931431566033, + "corners_xy": [ + [ + 974.7761840820314, + 914.5796508789065 + ], + [ + 1017.5194702148436, + 883.7464599609375 + ], + [ + 985.4195556640625, + 831.4888916015624 + ], + [ + 942.6104736328127, + 862.4670410156251 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.06637215662052325, + -0.23397940579450147, + -0.2661787298345349, + 0.9327364355879245 + ] + }, + { + "stamp_ns": 1789370931498142462, + "corners_xy": [ + [ + 975.1509399414064, + 916.763427734375 + ], + [ + 1018.3197021484376, + 886.8533935546875 + ], + [ + 986.9041748046874, + 834.0053100585938 + ], + [ + 943.6673583984375, + 864.0728149414064 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.06950745798152386, + -0.23595387880998475, + -0.2579843282181781, + 0.9343118145208724 + ] + }, + { + "stamp_ns": 1789370931531634265, + "corners_xy": [ + [ + 975.8786621093752, + 917.9412841796874 + ], + [ + 1019.243896484375, + 888.5614013671874 + ], + [ + 988.2468261718749, + 835.3853149414062 + ], + [ + 944.7456665039064, + 864.9467163085939 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.07128013529885475, + -0.23697460795863654, + -0.2534090555862298, + 0.9351716569921923 + ] + }, + { + "stamp_ns": 1789370931599159061, + "corners_xy": [ + [ + 976.1368408203126, + 920.2393798828126 + ], + [ + 1019.9390869140627, + 891.8221435546875 + ], + [ + 989.6243286132811, + 838.1390991210939 + ], + [ + 945.7327880859372, + 866.7662353515626 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.07459430152002257, + -0.23798134256309444, + -0.24532385094838383, + 0.9368120296668175 + ] + }, + { + "stamp_ns": 1789370931698376818, + "corners_xy": [ + [ + 976.0877685546874, + 923.356201171875 + ], + [ + 1020.3729858398439, + 896.2204589843751 + ], + [ + 991.0213623046876, + 841.8040771484375 + ], + [ + 946.619384765625, + 869.1585693359374 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.07875492548905712, + -0.24106107882927896, + -0.23421543603413855, + 0.9385256243215823 + ] + }, + { + "stamp_ns": 1789370931732324975, + "corners_xy": [ + [ + 976.7288208007811, + 924.3530883789061 + ], + [ + 1021.2028808593751, + 897.7369384765626 + ], + [ + 992.1870727539062, + 843.0687255859375 + ], + [ + 947.5731201171875, + 869.9779663085938 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.019315389834214883, + 0.31334383614890404, + -0.2561270481500509, + 0.9142436717139378 + ] + }, + { + "stamp_ns": 1789370931765026659, + "corners_xy": [ + [ + 977.1015014648438, + 925.3395385742189 + ], + [ + 1021.7630004882812, + 899.1652221679688 + ], + [ + 993.078125, + 844.3125610351561 + ], + [ + 948.2957153320311, + 870.7482299804688 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.017758502532213012, + 0.31409318769189365, + -0.25262127084541625, + 0.9149932232261642 + ] + }, + { + "stamp_ns": 1789370931832571149, + "corners_xy": [ + [ + 976.4909057617188, + 927.4225463867191 + ], + [ + 1021.4346313476561, + 902.0576782226564 + ], + [ + 993.3823852539062, + 846.7463378906247 + ], + [ + 948.2376098632814, + 872.4022827148438 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.015219909592459675, + 0.3161797814132457, + -0.24627896885826445, + 0.9160487812750909 + ] + }, + { + "stamp_ns": 1789370931864915434, + "corners_xy": [ + [ + 976.2988891601562, + 928.162841796875 + ], + [ + 1021.3336181640624, + 903.1165771484375 + ], + [ + 993.5557250976564, + 847.6354370117188 + ], + [ + 948.2985229492189, + 872.9996948242188 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.014766943240842656, + 0.31709378329707777, + -0.24375433936601248, + 0.9164154581967212 + ] + }, + { + "stamp_ns": 1789370931932052941, + "corners_xy": [ + [ + 977.3914794921874, + 929.7640991210938 + ], + [ + 1022.70166015625, + 905.4426879882812 + ], + [ + 995.4624633789064, + 849.637451171875 + ], + [ + 949.9594116210938, + 874.2561645507812 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.012509374407774517, + 0.318826740823427, + -0.2380761628088696, + 0.917340049049447 + ] + }, + { + "stamp_ns": 1789370931965527790, + "corners_xy": [ + [ + 977.6703491210936, + 930.2903442382811 + ], + [ + 1023.0899658203128, + 906.2533569335939 + ], + [ + 995.9970092773438, + 850.3397216796876 + ], + [ + 950.4229736328123, + 874.6978149414062 + ] + ], + "recorded_selected_quaternion_xyzw": [ + -0.01106802191087515, + 0.3192444937935004, + -0.23620565173579394, + 0.9176967593718423 + ] + } + ] + } + ] +} diff --git a/src/linkerhand_calibration/test/fixtures/o6_yaw_directional_114156.json.gz b/src/linkerhand_calibration/test/fixtures/o6_yaw_directional_114156.json.gz new file mode 100644 index 0000000..a918ed8 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o6_yaw_directional_114156.json.gz differ diff --git a/src/linkerhand_calibration/test/measured_capture_fixture.py b/src/linkerhand_calibration/test/measured_capture_fixture.py index c4ad5a3..3959afc 100644 --- a/src/linkerhand_calibration/test/measured_capture_fixture.py +++ b/src/linkerhand_calibration/test/measured_capture_fixture.py @@ -33,6 +33,8 @@ def reordered_profile(profile): approach_commands=tuple(vector(pose) for pose in spec.approach_commands)) for name, spec in profile.motion.joint_zero_references.items()} return replace(profile, key=ProfileKey("VIRTUAL", "right", "reordered_observations"), namespace="/virtual", + acquisition=replace(profile.acquisition, steady_extra_training_nodes={ + keys[key]: values for key, values in profile.acquisition.steady_extra_training_nodes.items()}), command=command, vision=replace(profile.vision, views=tuple(replace(view, name=cameras[view.name]) for view in profile.vision.views), extrinsic_reference_view=cameras[profile.vision.extrinsic_reference_view]), @@ -51,14 +53,19 @@ def measured_capture(tmp_path, *, nonlinear=True, reordered=False, layout="g20_r for name, joint in model.joints.items(): if joint.mimic_joint: offsets[name] = joint.mimic_multiplier*offsets.get(joint.mimic_joint, 0)+joint.mimic_offset + elif joint.kind != "fixed": + offsets.setdefault(name, 0.0) + command_field = f"command_{profile.command.unit}" + state_field = f"state_{profile.command.unit}" + vector_field = f"command_vector_{profile.command.unit}" specs, seeds = {}, {} for task in profile.motion.tasks: for name in task.joints: selected = [r for r in records if r.get("joint") == name and r["cycle"] == 0 and r["direction"] == "decreasing" - and r["command_u8"] == profile.command.baseline_values[task.command_index]] + and r[command_field] == profile.command.baseline_values[task.command_index]] seed = selected[0] - specs[name] = replace(profile.motion.joint_zero_references[name], command=tuple(seed["command_vector_u8"])) + specs[name] = replace(profile.motion.joint_zero_references[name], command=tuple(seed[vector_field])) seeds[name] = seed profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=3), motion=replace(profile.motion, joint_zero_references=specs), @@ -79,7 +86,8 @@ def measured_capture(tmp_path, *, nonlinear=True, reordered=False, layout="g20_r if nonlinear and name in profile.zero.passive_joints: joint = model.joints[name] channel = next(t.command_index for t in profile.motion.tasks if name in t.joints) - x = (profile.command.baseline_values[channel]-row["state_u8"][channel])/255 + travel = profile.command.maximum_values[channel]-profile.command.minimum_values[channel] + x = (profile.command.baseline_values[channel]-row[state_field][channel])/travel # A monotonic 0.35*x^2 additional passive angle, independently # generated in the physical link frame, never from fitted JSON. extra = np.eye(4) @@ -99,7 +107,7 @@ def measured_capture(tmp_path, *, nonlinear=True, reordered=False, layout="g20_r selected = [seeds[name]] if name in profile.measurement.cross_view_sources: selected.extend(r for r in records if r.get("observation_joint") == name and r["cycle"] == 0 - and r["direction"] == "decreasing" and r["command_u8"] == profile.command.baseline_values[task.command_index] + and r["direction"] == "decreasing" and r[command_field] == profile.command.baseline_values[task.command_index] and r.get("sample_phase", "sweep") == "sweep") arrival = "increasing" if spec.command[task.command_index] > spec.approach_commands[-1][task.command_index] else "decreasing" samples = [dict(seed, image_stamp_ns=1000+index*100+i, command_direction_by_index=[arrival]*profile.command.command_count) diff --git a/src/linkerhand_calibration/test/pose_branch_fixture.py b/src/linkerhand_calibration/test/pose_branch_fixture.py new file mode 100644 index 0000000..137f712 --- /dev/null +++ b/src/linkerhand_calibration/test/pose_branch_fixture.py @@ -0,0 +1,110 @@ +"""Explicit ideal-sensor premises for tests which do not exercise geometry. + +These helpers mock a *completed* motion confirmation; they do not test its +mathematical validity. Real candidate and full-flow tests supply that coverage. +""" + +import hashlib +import json + +from linkerhand_calibration.core.geometry.pnp import SquareTagPose, SquareTagPoseTracker +from linkerhand_calibration.core.geometry.tag_pose.branch_confirmation import BranchState + + +class IdealPoseTracker(SquareTagPoseTracker): + def accept_ideal_pose(self, role, pose, stamp_ns, *, motion_evidence_id=None): + with self._lock: + previous = self._branches.states.get(role) + frozen = previous.frozen if previous is not None else False + identity = motion_evidence_id or (previous.motion_evidence_id if previous is not None else None) + revision = previous.revision if previous is not None else (2 if identity else 0) + if identity and previous is not None and previous.motion_evidence_id is None: + revision = max(2, revision+1) + self._branches.states[role] = BranchState(observed_ns=stamp_ns, pose=pose, + revision=revision, frozen=frozen, status="tracking", reason="", motion_evidence_id=identity) + self.last_candidates_by_role[role] = () # No invented IPPE candidates. + self.last_candidate_diagnostics_by_role[role] = { + "observation_stamp_ns": stamp_ns, "branch_revision": revision, + "branch_status": "tracking", "branch_reason": "", + "branch_frozen": frozen, "branch_confirmation_count": 5, + "motion_evidence_verified": identity is not None, "motion_evidence_id": identity or "", + } + return pose, "" + + +def _ideal_motion_id(profile, view, role): + return hashlib.sha256(f"explicit-test-sensor:{profile.key.profile_id}:{view}:{role}".encode()).hexdigest() + + +def assume_motion_confirmation(host, joints): + """Inject only the already-completed approach premise into a timing test.""" + joints = tuple(joints) + task_key = host.profile.motion.joint_zero_references[joints[0]].task_key + initialization = host.branch_initialization + initialization.task_name, initialization.zero_joints = task_key, joints + initialization.session_epoch = host._observation_epoch + initialization.motion_version = max(1, host._segment_number) + initialization._invalid_reason = "" + views = {host.profile.measurement.measurements[name].view for name in joints} + views.update(host.profile.measurement.measurements[secondary].view for name in joints + if (secondary := host.profile.measurement.cross_view_sources.get(name)) is not None) + initialization._requested.update(views) + initialization.reports.update({view: {"status": "resolved", "reason": "explicit_test_sensor_premise"} + for view in views}) + + +def confirm_reference_rows(host, rows): + joints = set() + for row in rows: + view = row["view"] + tracker = host.trackers[view] + if not isinstance(tracker, IdealPoseTracker): + tracker = host.trackers[view] = IdealPoseTracker() + for side, evidence in row["pnp_observation_evidence"].items(): + role = evidence["tag_role"] + fixed = any(tag.role == role and tag.fixed_reference for candidate in host.profile.vision.views + if candidate.name == view for tag in candidate.tags) + pose = row[f"{side}_pose_common"] + tracker.accept_ideal_pose(role, SquareTagPose( + tuple(pose["quaternion_xyzw"]), tuple(pose["translation_xyz_m"]), 0.), row["image_stamp_ns"], + motion_evidence_id=None if fixed else _ideal_motion_id(host.profile, view, role)) + evidence["candidate_diagnostics"] = dict(tracker.last_candidate_diagnostics_by_role[role]) + child = row["pnp_observation_evidence"]["child"]["tag_role"] + joints.update(name for name, spec in host.profile.measurement.measurements.items() + if spec.view == view and spec.child_role == child + and name in host.profile.motion.joint_zero_references) + assume_motion_confirmation(host, sorted(joints)) + + +def add_ideal_motion_provenance(profile, joint, rows, *, session_epoch, motion_version, source_stamps): + """Add auditable synthetic premises for a checkpoint/transport-only test.""" + from linkerhand_calibration.core.domain.reference import joint_zero_moving_roles + from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 + task = profile.motion.joint_zero_references[joint].task_key + records = [] + for view in sorted({row["view"] for row in rows}): + base = {"task_name": task, "view": view, "zero_joints": [joint], + "session_epoch": session_epoch, "motion_version": motion_version, + "source_image_stamps": list(source_stamps), + "constraints": [{"test_fixture": "explicit_ideal_sensor_motion_premise"}]} + sources = [{"kind": "motion_branch_observation", "image_stamp_ns": stamp, + **{key: base[key] for key in ("task_name", "view", "zero_joints", "session_epoch", "motion_version")}} + for stamp in source_stamps] + base["source_frame_hashes"] = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in sources} + audit = {"kind": "motion_branch_initialization", "status": "resolved", **base, + "evidence_ids": {}, "evidence_payloads": {}} + for role_view, role in sorted(joint_zero_moving_roles(profile, joint)): + if role_view != view: + continue + payload = {**base, "tag_role": role} + identity = hashlib.sha256(json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() + audit["evidence_ids"][role], audit["evidence_payloads"][role] = identity, payload + for row in rows: + if row["view"] == view: + for item in row["pnp_observation_evidence"].values(): + if item["tag_role"] == role: + item["candidate_diagnostics"].update(branch_revision=2, + motion_evidence_verified=True, motion_evidence_id=identity) + records.extend(sources) + records.append(audit) + return records diff --git a/src/linkerhand_calibration/test/runtime_host_fixture.py b/src/linkerhand_calibration/test/runtime_host_fixture.py index aeecac6..a548166 100644 --- a/src/linkerhand_calibration/test/runtime_host_fixture.py +++ b/src/linkerhand_calibration/test/runtime_host_fixture.py @@ -13,14 +13,19 @@ from linkerhand_calibration.runtime.ros.parameters import load_runtime_parameter from linkerhand_calibration.runtime.runner_support import protected_inputs -def coordinator_fixture(tmp_path, *, model="o6", finalization=None): +def coordinator_fixture(tmp_path, *, model="o6", finalization=None, diagnostic_capture="", profile_transform=None, + resume_raw_samples_path=None): package = Path(__file__).resolve().parents[1] config = load_product_config(package / f"config/{model}_right_product.yaml", workspace=package.parents[1], check_can=False) profile = config.calibration_contract.typed_profile + if profile_transform is not None: + profile = profile_transform(profile) declared = next(iter(yaml.safe_load(config.calibration_config.read_text()).values()))["ros__parameters"] values = {**parameter_defaults(profile), **declared, "serial_number": "OFFLINE_COORDINATOR", "session_dir": str(tmp_path / model), + "diagnostic_capture": diagnostic_capture, + "resume_raw_samples_path": str(resume_raw_samples_path) if resume_raw_samples_path else "", "source_urdf_path": str(config.source_urdf), "camera_extrinsics_file": str(config.camera_extrinsics), **{key.replace("_sha256", "_expected_sha256"): value for key, value in protected_inputs(config).items()}} parameters = load_runtime_parameters(profile, values.__getitem__) diff --git a/src/linkerhand_calibration/test/test_adaptive_command_sampling.py b/src/linkerhand_calibration/test/test_adaptive_command_sampling.py new file mode 100644 index 0000000..f88d25f --- /dev/null +++ b/src/linkerhand_calibration/test/test_adaptive_command_sampling.py @@ -0,0 +1,94 @@ +"""A training plan saves stops without reading the independent validation set.""" + +from copy import deepcopy +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.domain.profile import observation_streams +from linkerhand_calibration.core.domain.sampling import command_nodes +from linkerhand_calibration.core.fitting.command_sampling import ( + build_sampling_plan, declared_training_nodes, reduced_knots, validate_sampling_plans, + MAXIMUM_REMOVAL_ERROR_RAD, +) +from linkerhand_calibration.product import load_product_config + + +def source_rows(profile, task, *, curved=False): + rows = [] + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for i, x in enumerate(np.linspace(task.start_value, task.end_value, 81)): + phase = (x-min(task.start_value, task.end_value))/abs(task.end_value-task.start_value) + angle = .8*phase + (.35*phase**3 if curved else 0) + for field, name, spec in observation_streams(profile, task): + rows.append({"kind": "joint_sample", field: name, "view": spec.view, + "task_name": task.key, "image_stamp_ns": 1000000+len(rows), "cycle": cycle, + "direction": direction, "sample_phase": "sweep", "attempt": 1, + f"feedback_{profile.command.unit}": float(x), + "relative_quaternion_xyzw": Rotation.from_rotvec([0, 0, angle]).as_quat().tolist()}) + return rows + + +@pytest.mark.parametrize("name", ["g20", "l6", "o6", "o12"]) +def test_all_models_reduce_linear_training_and_keep_validation_unchanged(name): + profile = load_product_config(Path(__file__).resolve().parents[1]/f"config/{name}_right_product.yaml", + check_can=False).calibration_contract.typed_profile + task = profile.motion.tasks[0] + rows = source_rows(profile, task) + plan = build_sampling_plan(profile, task, rows) + assert 3 <= len(plan["training_nodes"]) <= 4 + assert len(plan["training_nodes"]) < len(command_nodes(profile, task)) + assert tuple(plan["validation_nodes"]) == command_nodes(profile, task, holdout=True) + assert validate_sampling_plans(profile, [*rows, plan]) == {task.key: plan} + changed = deepcopy(rows) + for row in changed: + if row["cycle"] == 3: + row["relative_quaternion_xyzw"] = [1, 0, 0, 0] + row[f"feedback_{profile.command.unit}"] += 100 + assert build_sampling_plan(profile, task, changed) == plan + assert len(build_sampling_plan(profile, task, source_rows(profile, task, curved=True))["training_nodes"]) > 3 + missing = [row for row in rows if row["cycle"] != 1] + assert tuple(build_sampling_plan(profile, task, missing)["training_nodes"]) == command_nodes(profile, task) + changed[0]["relative_quaternion_xyzw"] = [0, 0, .1, .99498743710662] + with pytest.raises(ValueError, match="sampling_training_evidence_changed"): + validate_sampling_plans(profile, [*changed, plan]) + damaged = {**plan, "training_nodes": [0, 255]} + with pytest.raises(ValueError, match="sampling_plan_hash_changed"): + declared_training_nodes(profile, task, [damaged]) + + +def test_shortest_plan_respects_each_branch_and_mandatory_nodes(): + grid = np.linspace(0, 1, 9) + curves = np.array([grid, grid + .03*np.sin(6*grid), grid**2]) + selected = reduced_knots(grid, curves, {0, 4, 8}) + assert {0, .5, 1} <= set(selected) + for curve in curves: + actual = np.interp(grid, selected, np.interp(selected, grid, curve)) + assert max(abs(actual-curve)) <= MAXIMUM_REMOVAL_ERROR_RAD + + +@pytest.mark.parametrize("layout", ["l6_right_8", "o12_right_16"]) +def test_reduced_static_training_still_rejects_wrong_independent_validation(layout): + from dataclasses import replace + from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings + from test_dual_mapping import independent_steady_fixture + profile, fit, name, rows, _ = independent_steady_fixture(layout) + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate")) + task = profile.motion.tasks[0] + plan = build_sampling_plan(profile, task, source_rows(profile, task)) + assert len(plan["training_nodes"]) < len(command_nodes(profile, task)) + selected = [dict(row, cycle={0: 4, 3: 5}[row["cycle"]], + steady_training_nodes=plan["training_nodes"], sampling_plan_sha256=plan["plan_sha256"]) + for row in rows if row["cycle"] == 3 + or (row["cycle"] == 0 and row["steady_target"] in plan["training_nodes"])] + assert fit_command_mappings(profile, fit, {name: selected}).command_mappings + for row in selected: + if row["cycle"] == 5: + row["relative_quaternion_xyzw"] = (Rotation.from_rotvec([0, 0, .09]) + * Rotation.from_quat(row["relative_quaternion_xyzw"])).as_quat() + with pytest.raises(ValueError, match="steady_command_holdout_failed"): + fit_command_mappings(profile, fit, {name: selected}) + assert not fit.command_mappings diff --git a/src/linkerhand_calibration/test/test_affine_mimic_export.py b/src/linkerhand_calibration/test/test_affine_mimic_export.py new file mode 100644 index 0000000..994b3c9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_affine_mimic_export.py @@ -0,0 +1,103 @@ +"""Keep standalone URDF coupling while JSON retains the measured nonlinear path.""" + +from copy import deepcopy +import json + +import numpy as np +import pytest + +from test_independent_urdf_export import nonlinear_release, assert_robot_state_publisher_angles +from linkerhand_calibration.core.fitting.coupling import fit_range_bounded_mimic +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( + rebuild_urdf_from_json, serialize_urdf_correction, validate_json_urdf_correspondence, +) + + +@pytest.fixture +def affine_release(nonlinear_release): + source, rights, original_plan, payload, path = nonlinear_release + model = UrdfKinematicModel(source) + parent, child = model.joints["pitch"], model.joints["ip"] + x = np.tile(payload["joints"]["pitch"]["angle_rad"], 3) + y = np.tile(payload["joints"]["ip"]["angle_rad"], 3) + fitted = fit_range_bounded_mimic("pitch", "ip", x, y, cycles=np.repeat([0, 1, 2], 256), + parent_range=(parent.lower, parent.upper), child_range=(child.lower, child.upper)) + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=original_plan.source_sha256, zero_offsets_rad={}, authorized_fields=rights, + fitted_mimics_output={"ip": fitted}) + payload = deepcopy(payload) + payload["urdf_correction"] = serialize_urdf_correction(plan) + path.write_text(json.dumps(payload)) + return source, rights, plan, payload, path + + +def test_near_zero_limits_do_not_flatten_affine_motion_and_json_stays_nonlinear(affine_release, tmp_path): + source, rights, plan, payload, path = affine_release + output = rebuild_urdf_from_json(path, source, tmp_path/"affine.urdf", + authorized_fields=rights, expected_profile_id=payload["profile_id"]) + validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) + assert output.read_bytes() == plan.write(source, tmp_path/"direct.urdf").read_bytes() + assert payload["urdf_correction"]["schema_version"] == 3 + original, model = UrdfKinematicModel(source), UrdfKinematicModel(output) + passive = model.joints["ip"] + assert passive.mimic_joint == "pitch" + assert passive.mimic_multiplier > 1.75 + assert 0 < passive.mimic_offset < .002 + for name, joint in original.joints.items(): + assert (model.joints[name].lower, model.joints[name].upper) == (joint.lower, joint.upper) + np.testing.assert_array_equal(model.joints[name].origin, joint.origin) + for command in (0, 32, 128, 255): + explicit = {name: row["angle_rad"][command] for name, row in payload["joints"].items()} + assert model.resolve_angles(explicit, independent_mimic_angles=True) == explicit + preview = model.resolve_angles({"pitch": explicit["pitch"]}) + assert preview["ip"] == pytest.approx(passive.mimic_multiplier*explicit["pitch"]+passive.mimic_offset) + assert abs(preview["ip"]-explicit["ip"]) < np.deg2rad(4) + for angle in np.linspace(model.joints["pitch"].lower, model.joints["pitch"].upper, 257): + value = model.resolve_angles({"pitch": angle})["ip"] + assert passive.lower-1e-12 <= value <= passive.upper+1e-12 + + +@pytest.mark.parametrize("policy", ["missing", "unknown", "legacy"]) +def test_nonzero_approximation_requires_explicit_serialized_policy(affine_release, tmp_path, policy): + source, rights, _, payload, path = affine_release + payload = deepcopy(payload) + correction = payload["urdf_correction"] + if policy == "missing": + del correction["mimic_policy"] + elif policy == "unknown": + correction["mimic_policy"] = "assumed_physical_zero" + else: + correction["schema_version"] = 1 + del correction["mimic_policy"], correction["joint_angle_source"] + path.write_text(json.dumps(payload)) + output = tmp_path/"invalid.urdf" + with pytest.raises(ValueError): + rebuild_urdf_from_json(path, source, output, authorized_fields=rights, + expected_profile_id=payload["profile_id"]) + assert not output.exists() + + +def test_standard_ros_parent_input_drives_passive_joint(affine_release, tmp_path): + assert_robot_state_publisher_angles(affine_release, tmp_path, use_mimic=True) + + +@pytest.mark.parametrize("parent_range,child_range", [((-.002, .6), (-.003, 1.2)), ((-.6, .002), (-.003, 1.2))]) +def test_range_bounded_fit_reports_real_residuals_without_rewriting_training_data(parent_range, child_range): + sign = 1 if parent_range[1] == .6 else -1 + x = sign*np.tile(np.linspace(0, .6, 80), 3) + y = 1.71*np.abs(x)+.39*x*x + original = y.copy() + cycles = np.repeat([0, 1, 2], 80) + fit = fit_range_bounded_mimic("parent", "child", x, y, cycles=cycles, + parent_range=parent_range, child_range=child_range) + assert sign*fit.multiplier > 1.7 + assert fit.offset_observed is False # The approximation intercept is not a physical datum. + np.testing.assert_array_equal(y, original) + np.testing.assert_allclose(fit.residuals_rad, fit.multiplier*x+fit.offset_rad-y) + for endpoint in parent_range: + assert child_range[0]-1e-10 <= fit.multiplier*endpoint+fit.offset_rad <= child_range[1]+1e-10 + with pytest.raises(ValueError, match="holdout is forbidden"): + fit_range_bounded_mimic("parent", "child", x, y, cycles=np.repeat([0, 1, 3], 80), + parent_range=parent_range, child_range=child_range) diff --git a/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py b/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py new file mode 100644 index 0000000..afbe185 --- /dev/null +++ b/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py @@ -0,0 +1,99 @@ +"""Shared v3 generation for every product using independent synthetic poses. + +Synthetic geometry/limits are explicit fixture truth, not hardware evidence. +""" + +from dataclasses import replace +import hashlib +import json + +import pytest + +from measured_capture_fixture import measured_capture +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper +from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json + + +@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"]) +def test_separate_motion_mapping_json_urdf_and_reader(layout, tmp_path, monkeypatch): + from linkerhand_calibration.runtime.artifacts import finalization + from linkerhand_calibration.core.fitting import mimic_approximation + from linkerhand_calibration.core.urdf.source_mimic import validate_source_mimic_elements + def forbidden_fit(*args, **kwargs): + pytest.fail("formal measured calibration must not optimize mimic parameters") + monkeypatch.setattr(mimic_approximation, "fit_mimic_approximations", forbidden_fit) + monkeypatch.setattr(mimic_approximation, "fit_bounded_mimic_approximations", forbidden_fit) + validators = [] + original_validator = finalization.FrozenTagArtifactValidator + def record_validator(*args, **kwargs): + from test_final_image_holdout import independent_pixels + validator = original_validator(*args, **kwargs) + validator = replace(validator, require_image_holdout=True, + image_observations=independent_pixels(validator.observations), + command_image_observations=independent_pixels(validator.command_observations)) + validators.append(validator) + return validator + monkeypatch.setattr(finalization, "FrozenTagArtifactValidator", record_validator) + profile, source, records, _ = measured_capture(tmp_path, layout=layout, + nonlinear=layout != "o12_right_16") + if layout == "o12_right_16": + from linkerhand_calibration.profiles import load_bundled_hand_profile + assumptions = load_bundled_hand_profile(layout).zero.cad_zero_assumptions + profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions=assumptions, + known_baseline_geometry={name: value for name, value in profile.zero.known_baseline_geometry.items() + if name not in assumptions})) + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate")) + records = [dict(row, cycle={0: 4, 3: 5}[row["cycle"]]) if row.get("sample_phase") == "steady" else row + for row in records if row.get("sample_phase") != "steady" or row["cycle"] in {0, 3}] + from linkerhand_calibration.core.fitting.command_sampling import build_sampling_plan + plans = {task.key: build_sampling_plan(profile, task, records) for task in profile.motion.tasks} + records = [dict(row, steady_training_nodes=plans[row["task_name"]]["training_nodes"], + sampling_plan_sha256=plans[row["task_name"]]["plan_sha256"]) + if row.get("sample_phase") == "steady" else row for row in records + if row.get("sample_phase") != "steady" or row["cycle"] != 4 + or row["steady_target"] in plans[row["task_name"]]["training_nodes"]] + records.extend(plans.values()) + hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} + hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() + phases = [] + payload, fit, result = finalize_profile_session(profile=profile, source_urdf=source, + records=records, protected_inputs=hashes, serial_number="SYNTHETIC_ONLY", session_dir=tmp_path/"session", + standard_loader=lambda _: None, phase_changed=phases.append) + assert phases == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] + assert payload["schema_version"] == 3 + assert payload["urdf_correction"]["schema_version"] == 4 + assert payload["urdf_correction"]["mimic_policy"] == "preserve_source_mimic" + assert not fit.mimic_approximations + validate_source_mimic_elements(source, result.path) + assert all(row["training_cycles"] == [4] and row["holdout_cycle"] == 5 + for row in fit.command_holdout_metrics.values()) + assert payload["urdf_correction"]["source_urdf_sha256"] == hashes["source_urdf_sha256"] + accepted = json.loads(result.staged_release.validation_json) + assert accepted["command_holdout_cycle"] == 5 + assert accepted["final_file_image_holdout_verified"] is True + assert accepted["final_file_image_holdout_policy"] == "independent_measurement_and_physical_consistency_v2" + assert set(accepted["final_file_image_holdout"]) == {"feedback", "command"} + assert accepted["urdf_rebuilt_from_json"] is True + assert accepted["cad_zero_assumptions"] == dict(profile.zero.cad_zero_assumptions) + for name in profile.zero.cad_zero_assumptions: + assert fit.zero_offsets_rad[name] == 0 + assert fit.zero_method_by_joint[name] == "assumed_source_cad_zero" + mapper = load_unified_mapper(result.staged_release.calibration_json) + assert max(abs(v) for v in mapper.map_positions(profile.command.baseline_values)) < 1e-10 + rebuilt = rebuild_urdf_from_json(result.staged_release.calibration_json, source, tmp_path/"rebuilt.urdf", + authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) + assert rebuilt.read_bytes() == result.path.read_bytes() + assert hashlib.sha256(source.read_bytes()).hexdigest() == hashes["source_urdf_sha256"] + # Editing JSON and its report together cannot claim different corrections + # for an unchanged URDF, even when the command tables still agree. + report_path = result.path.parent/"calibration_report.json" + report = json.loads(report_path.read_text()) + edits = payload["urdf_correction"]["joint_patches"] + name = next(iter(edits)) + edits[name]["origin_rpy"] = "0 0 0.123" + report["urdf_correction"] = payload["urdf_correction"] + result.staged_release.calibration_json.write_text(json.dumps(payload)) + report_path.write_text(json.dumps(report)) + with pytest.raises(ValueError, match="calibration_json_correction_differs_from_urdf"): + validators[0](result.staged_release.calibration_json, result.path) diff --git a/src/linkerhand_calibration/test/test_all_view_image_acceptance.py b/src/linkerhand_calibration/test/test_all_view_image_acceptance.py new file mode 100644 index 0000000..aabe1f9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_all_view_image_acceptance.py @@ -0,0 +1,103 @@ +"""Final image evidence includes other motions, with no invented 3-D poses.""" + +from copy import deepcopy +from dataclasses import replace +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.runtime.artifacts.evidence import _replay_directions +from linkerhand_calibration.runtime.artifacts.image_evidence import ImageEvidence +from linkerhand_calibration.runtime.image_capture import IMAGE_OBSERVATION_POLICY +from test_final_image_sources import scene + + +def source_scene(input_kind="feedback"): + config, records, observations = scene() + profile = config.calibration_contract.typed_profile + tasks = {task.key: task for task in profile.motion.tasks} + mappings = {joint: SimpleNamespace(motor_index=task.command_index) + for task in tasks.values() for joint in task.joints} + cycle = 3 if input_kind == "feedback" else 5 + phase = "sweep" if input_kind == "feedback" else "steady" + frames = {f"{r['view']}:{r['image_stamp_ns']}": r for r in records if r['kind'] == 'pnp_candidate_frame'} + raw, primary = [], [] + for observation in observations: + frame = frames[observation.sample_id] + frame['cycle'] = cycle + history = ['increasing'] * profile.command.command_count + history[tasks[frame['task_name']].command_index] = frame['direction'] + values = list(observation.sdk_values) + values[0] = 120.25 # Exposure-time feedback is not an integer command. + image = dict(kind='image_observation_frame', policy=IMAGE_OBSERVATION_POLICY, + view=frame['view'], image_stamp_ns=frame['image_stamp_ns'], task_name=frame['task_name'], + cycle=cycle, direction=frame['direction'], attempt=1, sample_phase=phase, + tags=deepcopy(frame['roles']), command_unit=profile.command.unit, + command_vector=[120] * profile.command.command_count, feedback_vector=values, + command_direction_by_index=history, state_image_sync_error_ns=10_000_000, + camera_matrix=frame['camera_matrix'], camera_matrix_source='CameraInfo.P[:3,:3]', + input_is_rectified=True) + sdk = image['feedback_vector' if input_kind == 'feedback' else 'command_vector'] + directions = _replay_directions(mappings, image, tasks[image['task_name']]) + primary.append(replace(observation, cycle=cycle, sdk_values=tuple(sdk), sdk_directions_by_joint=directions)) + raw.append(image) + records.append(dict(kind='scan_unit_complete', task_name=frame['task_name'], cycle=cycle, + direction=frame['direction'], attempt=1, passed=True)) + # ID7 is on the distal link. Its pixels during the flexion task must be + # checked even though that task's primary camera is front. + extra = deepcopy(next(row for row in raw if row['view'] == 'top')) + extra.update(image_stamp_ns=extra['image_stamp_ns']+1, task_name='thumb_pitch_ip_front') + for point in extra['tags']['thumb_yaw']['corners_xy']: + point[0] += 8 + records.extend(raw + [extra]) + records.append(dict(kind='scan_unit_complete', task_name=extra['task_name'], cycle=cycle, + direction=extra['direction'], attempt=1, passed=True)) + return config, records, primary, mappings, extra + + +def prepare(config, records, primary, mappings, input_kind): + evidence = ImageEvidence(config.calibration_contract.typed_profile, records, + camera_extrinsics_file=config.camera_extrinsics, + protected_inputs={'camera_extrinsics_sha256': config.camera_extrinsics_sha256}) + return evidence.all_view_holdout(primary, mappings=mappings, input_kind=input_kind, + holdout_cycle=3 if input_kind == 'feedback' else 5) + + +@pytest.mark.parametrize('input_kind', ['feedback', 'command']) +def test_other_motion_pixels_reach_the_final_check_without_pnp(input_kind): + config, records, primary, mappings, extra = source_scene(input_kind) + images = prepare(config, records, primary, mappings, input_kind) + assert len(images) == 3 + added = next(row for row in images if row.sample_id == f"top:{extra['image_stamp_ns']}") + assert added.role == 'thumb_yaw' + assert added.task_name == 'thumb_pitch_ip_front' + assert added.corners_xy == tuple(map(tuple, extra['tags']['thumb_yaw']['corners_xy'])) + assert added.sdk_values[0] == (120.25 if input_kind == 'feedback' else 120) + assert dict(added.directions)['rh_thumb_cmc_yaw'] == extra['command_direction_by_index'][1] + assert not any(r['kind'] == 'pnp_candidate_frame' and r['image_stamp_ns'] == extra['image_stamp_ns'] for r in records) + + +@pytest.mark.parametrize('mutation', ['pixels', 'sdk', 'policy', 'unit', 'direction', 'missing', 'duplicate', 'skew', 'nonfinite']) +def test_conflicting_or_missing_primary_raw_evidence_is_rejected(mutation): + config, records, primary, mappings, extra = source_scene() + row = next(r for r in records if r['kind'] == 'image_observation_frame' and r['view'] == 'top' and r is not extra) + if mutation == 'pixels': row['tags']['thumb_yaw']['corners_xy'][0][0] += 5 + elif mutation == 'sdk': row['feedback_vector'][0] += .5 + elif mutation == 'policy': row['policy'] = 'unknown' + elif mutation == 'unit': row['command_unit'] = 'rad' + elif mutation == 'direction': row['command_direction_by_index'] = ['increasing'] + elif mutation == 'missing': records.remove(row) + elif mutation == 'skew': row['state_image_sync_error_ns'] = -1 + elif mutation == 'nonfinite': row['feedback_vector'][0] = float('nan') + else: records.append({**row, 'feedback_vector': [1] * 6}) + with pytest.raises(ValueError, match='final_image'): + prepare(config, records, primary, mappings, 'feedback') + + +@pytest.mark.parametrize('mutation', ['training', 'failed_attempt', 'missing_feedback']) +def test_unavailable_or_non_holdout_auxiliary_images_do_not_become_measurements(mutation): + config, records, primary, mappings, extra = source_scene() + if mutation == 'training': extra['cycle'] = 2 + elif mutation == 'failed_attempt': extra['attempt'] = 2 + else: extra['feedback_vector'] = None + assert len(prepare(config, records, primary, mappings, 'feedback')) == 2 diff --git a/src/linkerhand_calibration/test/test_all_view_image_capture.py b/src/linkerhand_calibration/test/test_all_view_image_capture.py new file mode 100644 index 0000000..66795a8 --- /dev/null +++ b/src/linkerhand_calibration/test/test_all_view_image_capture.py @@ -0,0 +1,71 @@ +"""Other cameras retain raw pixels without posing as primary joint samples.""" + +from dataclasses import replace + +import numpy as np +import pytest + +from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +def test_non_task_camera_records_measured_corners_without_a_pose(tmp_path, monkeypatch, model): + host, clock = coordinator_fixture(tmp_path, model=model) + try: + ready(host, clock) + assert host.start().success + host.execution.session.phase = Phase.SWEEP + task = host.profile.motion.tasks[0] + host._motion = MotionCommand("sweep", host.profile.command.baseline_values, 1., + task_key=task.key, command_index=task.command_index, cycle=2, direction="decreasing") + view = next(view for view in host.profile.vision.views if view.name != task.view) + tag = next(tag for tag in view.tags if not tag.fixed_reference) + points = ((200., 200.), (240., 200.), (240., 240.), (200., 240.)) + # Camera-derived geometry may be unresolved in this other motion. + # Raw measured pixels must survive without inventing a Tag pose. + monkeypatch.setattr(host.capture, "consume", lambda *a, **k: ((), None)) + host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), ( + TagDetection(tag.tag_id, points, 0, 100.),))) + host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), ( + TagDetection(tag.tag_id, points, 0, 100.),))) + rows = [r for r in host.raw_records if r["kind"] == "image_observation_frame"] + assert len(rows) == 1 + row = rows[0] + assert row["task_name"] == task.key and row["view"] == view.name + assert (row["cycle"], row["direction"], row["sample_phase"]) == (2, "decreasing", "sweep") + np.testing.assert_array_equal(row["tags"][tag.role]["corners_xy"], points) + assert "pose" not in row["tags"][tag.role] + assert not any(r["kind"] == "joint_sample" for r in host.raw_records) + assert host.execution.session.phase == Phase.SWEEP + assert not clock.positions # Observing an additional view schedules no motion. + finally: + host.close() + + +def test_raw_images_keep_detection_quality_and_stable_window_boundaries(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + try: + ready(host, clock) + assert host.start().success + host.execution.session.phase = Phase.SWEEP + task = host.profile.motion.tasks[0] + host._motion = MotionCommand("steady", host.profile.command.baseline_values, 1., + task_key=task.key, command_index=task.command_index, cycle=5, direction="increasing") + view = next(view for view in host.profile.vision.views if view.name == task.view) + tag = next(tag for tag in view.tags if not tag.fixed_reference) + points = ((200., 200.), (240., 200.), (240., 240.), (200., 240.)) + monkeypatch.setattr(host.capture, "consume", lambda *a, **k: ((), None)) + observation = DetectionInput(view.name, host.ports.clock_ns(), (TagDetection(tag.tag_id, points, 0, 100.),)) + host.receive_detections(observation) + assert not any(r["kind"] == "image_observation_frame" for r in host.raw_records) + host._steady_capture_after_ns = host.ports.clock_ns() + rejected = replace(observation.detections[0], hamming=100) + host.receive_detections(replace(observation, detections=(rejected,))) + row = next(r for r in host.raw_records if r["kind"] == "image_observation_frame") + assert not row["tags"] + assert row["cycle"] == 5 and row["session_epoch"] == host._observation_epoch + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_axis_line_gauge.py b/src/linkerhand_calibration/test/test_axis_line_gauge.py new file mode 100644 index 0000000..9e53d0e --- /dev/null +++ b/src/linkerhand_calibration/test/test_axis_line_gauge.py @@ -0,0 +1,59 @@ +"""Absolute zero must be invariant to reference points on the same axis line.""" + +from pathlib import Path + +import numpy as np +import pytest +from scipy.optimize import brentq +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.session import compile_spatial_profile +from linkerhand_calibration.core.fitting.spatial_solver.residuals import ObservationGeometry +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.profiles import load_bundled_hand_profile + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "g20_right_19", "o12_right_16"]) +@pytest.mark.parametrize("view", ["oblique_end_on", "side"]) +def test_physical_zero_is_recovered_for_any_axis_reference_point(layout, view): + package = Path(__file__).resolve().parents[1] + model_name = layout.split("_")[0] + source, = (package / "urdf" / f"{model_name}_right").glob("*.urdf") + model = UrdfKinematicModel(source) + spatial = compile_spatial_profile(load_bundled_hand_profile(layout)) + geometry = ObservationGeometry(spatial, model, {}, {}, {}) + common = Rotation.from_euler("xyz", [.2, -.15, .4]) + translation = np.array([.4, -.2, .8]) + truth = .07 + + def line(joint, parent, angle): + axis, point = model.axis_line(joint, zero_offsets={parent: angle}, + joint_angles={}, independent_mimic_angles=True) + return common.apply(axis), common.apply(point)+translation + + assert spatial.phase_parent_joint + for child, parent in spatial.phase_parent_joint.items(): + parent_axis, parent_point = line(parent, parent, truth) + child_axis, child_point = line(child, parent, truth) + assert abs(parent_axis @ child_axis) == pytest.approx(1., abs=1e-8) + transverse = np.cross(parent_axis, np.eye(3)[np.argmin(abs(parent_axis))]) + transverse /= np.linalg.norm(transverse) + normal = parent_axis + .3*transverse if view == "oblique_end_on" else transverse + normal /= np.linalg.norm(normal) + recovered = [] + for parent_shift, child_shift in [(0., 0.), (.03, -.09), (-.07, .04)]: + observed_parent = parent_point + parent_shift*parent_axis + observed_child = child_point + child_shift*child_axis + + def residual(angle): + predicted_axis, predicted_parent = line(parent, parent, angle) + _, predicted_child = line(child, parent, angle) + # The predicted lines also admit arbitrary reference points. + predicted_parent = predicted_parent + .08*predicted_axis + predicted_child = predicted_child - .06*predicted_axis + return geometry.phase_error(predicted_axis, predicted_parent, predicted_child, + parent_axis, observed_parent, observed_child, normal) + + assert residual(truth) == pytest.approx(0., abs=1e-9), (layout, parent, child) + recovered.append(brentq(residual, -.25, .25)) + assert recovered == pytest.approx([truth]*3, abs=1e-9), (layout, parent, child) diff --git a/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py b/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py new file mode 100644 index 0000000..9889515 --- /dev/null +++ b/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py @@ -0,0 +1,44 @@ +"""Nonlinear measured motion must not make the URDF fallback exceed limits.""" + +import numpy as np +import pytest + +from linkerhand_calibration.core.fitting.coupling import fit_standard_mimic +from linkerhand_calibration.core.fitting.mimic_approximation import shared_zero_multiplier_bounds + + +@pytest.mark.parametrize("parent,child,expected", [ + ((0, 2), (0, 1), (0, .5)), + ((-2, 0), (0, 1), (-.5, 0)), + ((-2, 3), (-1, 2), (-1/3, .5)), +]) +def test_feasible_multiplier_respects_both_endpoints(parent, child, expected): + bounds = shared_zero_multiplier_bounds(parent, child) + assert bounds == pytest.approx(expected) + for slope in bounds: + for endpoint in parent: + assert child[0]-1e-12 <= slope*endpoint <= child[1]+1e-12 + + +def test_nonlinear_fit_is_constrained_without_changing_observations(): + x = np.tile(np.linspace(0, 2, 80), 3) + y = x - .25*x*x + original = y.copy() + cycles = np.repeat([0, 1, 2], 80) + args = dict(source_joint="parent", target_joint="child", active_rad=x, + passive_rad=y, cycles=cycles, offset_rad=0) + unconstrained = fit_standard_mimic(**args) + bounded = fit_standard_mimic(**args, + multiplier_bounds=shared_zero_multiplier_bounds((0, 2), (0, 1))) + assert unconstrained.multiplier*2 > 1 + assert bounded.multiplier == pytest.approx(.5, abs=1e-8) + assert bounded.offset_rad == 0 + np.testing.assert_array_equal(y, original) + np.testing.assert_allclose(bounded.residuals_rad, bounded.multiplier*x-y) + with pytest.raises(ValueError, match="holdout is forbidden"): + fit_standard_mimic(**{**args, "cycles": np.repeat([0, 1, 3], 80)}, multiplier_bounds=(0, .5)) + + +def test_impossible_shared_zero_range_is_rejected(): + with pytest.raises(ValueError, match="excludes_shared_zero"): + shared_zero_multiplier_bounds((0, 2), (1, 2)) diff --git a/src/linkerhand_calibration/test/test_branch_initialization.py b/src/linkerhand_calibration/test/test_branch_initialization.py new file mode 100644 index 0000000..cfa0a35 --- /dev/null +++ b/src/linkerhand_calibration/test/test_branch_initialization.py @@ -0,0 +1,225 @@ +"""Declared arrival evidence, source identity and coordinator cancellation.""" + +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import asdict, replace +from threading import Event +import json + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime import coordinator as coordinator_module +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization +from linkerhand_calibration.runtime.inputs import DetectionInput +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 +from test_diagnostic_coordinator import projected_frame, start_diagnostic, TERMINAL_PHASES +from test_motion_branch_evidence import motion_frames +from measured_capture_fixture import reordered_profile + + +def arrival(profile): + task = profile.motion.tasks[0] + name = task.joints[0] + reference = profile.motion.joint_zero_references[name] + common = dict(task_key=task.key, command_index=task.command_index) + approach = MotionCommand("zero_approach", reference.command, 1., + reference_joints=(name,), **common) + zero = MotionCommand("joint_zero", reference.command, 1., zero_joints=(name,), **common) + measurement = profile.measurement.measurements[name] + rows = [] + samples = motion_frames() + for i, frame in enumerate(samples): + command = list(reference.command) + index = task.command_index + command[index] = reference.approach_commands[-1][index] + i / (len(samples)-1) * ( + reference.command[index] - reference.approach_commands[-1][index]) + tags = {} + for role, item in zip((measurement.parent_role, measurement.child_role), frame.roles): + tags[role] = {"reprojection_valid_candidates": [asdict(pose) for pose in item.candidates], + "selected_pose": asdict(item.candidates[-1]), + "candidate_diagnostics": {"observation_stamp_ns": frame.stamp_ns, + "branch_frozen": item.frozen}} + rows.append(dict(kind="motion_branch_observation", task_name=task.key, + view=measurement.view, zero_joints=[name], image_stamp_ns=frame.stamp_ns, + command_vector=command, feedback_vector=command.copy(), tags=tags, + session_epoch=1, motion_version=7)) + return approach, zero, rows + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19", + "virtual_o6_right_8", "virtual_g20_right_19"]) +def test_all_models_bind_motion_before_zero_to_the_actual_source_version(layout): + profile = load_bundled_hand_profile(layout.removeprefix("virtual_")) + if layout.startswith("virtual_"): + profile = reordered_profile(profile) + approach, zero, rows = arrival(profile) + # This legacy fixture contains only 3D candidates, no captured pixels. + # It verifies the historical evidence/version binding separately from + # the production image path exercised by the coordinator tests below. + owner = BranchInitialization(profile, image_geometry=False) + owner.begin(approach, session_epoch=1, motion_version=7) + for row in rows: + owner.observe(row) + owner.begin(zero, session_epoch=1, motion_version=8) + request = owner.request(rows[0]["view"], zero, session_epoch=1, motion_version=8) + assert request.motion_version == 7 + result = solve_initialization(request) + assert result.resolved, result + model, report = owner.accept(request, result) + assert owner.ready(zero) and model.evidence_ids + expected = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in rows} + assert report["source_frame_hashes"] + assert all(expected[key] == value for key, value in report["source_frame_hashes"].items()) + assert set(report["source_frame_hashes"]) == {str(s) for s in report["source_image_stamps"]} + assert not set(report["source_image_stamps"]) - {row["image_stamp_ns"] for row in rows} + + +def test_only_last_arrival_segment_and_complete_holding_state_can_authorize_zero(): + profile = load_bundled_hand_profile("o6_right_8") + approach, zero, rows = arrival(profile) + owner = BranchInitialization(profile) + owner.begin(approach, session_epoch=1, motion_version=6) + for row in rows: + owner.observe(dict(row, motion_version=6)) + owner.begin(approach, session_epoch=1, motion_version=7) + # Prior, stale-version images and images without SDK state cannot survive + # the final arrival segment or be substituted for its physical evidence. + for row in rows: + owner.observe(dict(row, motion_version=6)) + owner.observe(dict(row, feedback_vector=None)) + wrong_pose = deepcopy(row) + wrong_pose["command_vector"][0] -= 10 + owner.observe(wrong_pose) + owner.begin(zero, session_epoch=1, motion_version=8) + request = owner.request("top", zero, session_epoch=1, motion_version=8) + assert not request.frames and not solve_initialization(request).resolved + assert not owner.ready(zero) + + +def test_movement_of_a_held_joint_invalidates_motion_evidence(): + profile = load_bundled_hand_profile("o6_right_8") + approach, zero, rows = arrival(profile) + owner = BranchInitialization(profile) + owner.begin(approach, session_epoch=1, motion_version=7) + for i, row in enumerate(rows): + if i == len(rows)//2: + row["feedback_vector"][0] -= 5 + owner.observe(row) + owner.begin(zero, session_epoch=1, motion_version=8) + request = owner.request("top", zero, session_epoch=1, motion_version=8) + result = solve_initialization(request) + assert result.reason == "motion_held_feedback_changed" and not result.resolved + owner.accept(request, result) + assert not owner.ready(zero) + assert "top:joints=rh_thumb_cmc_yaw" in owner.error() + + +def before_initialization(host, clock): + """Run real arrival motion but stop before the first zero image callback.""" + for _ in range(8000): + clock.now += .04 + feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + host.tick() + assert host.execution.session.phase not in TERMINAL_PHASES, host.reason + if host._motion is not None and host._motion.phase == "joint_zero": + return projected_frame(host, "top", feedback) + task = next((t for t in host.profile.motion.tasks + if host._motion is not None and t.key == host._motion.task_key), None) + for view in (task.view,) if task is not None else host.required_observation_views: + host.receive_detections(projected_frame(host, view, feedback)) + pytest.fail("arrival never completed") + + +@pytest.mark.parametrize("change", ["abort", "motion_version"]) +def test_geometry_solves_outside_state_lock_and_cannot_commit_after_invalidation(tmp_path, monkeypatch, change): + host, clock = start_diagnostic(tmp_path, monkeypatch) + entered, release = Event(), Event() + original = coordinator_module.solve_initialization + try: + frame = before_initialization(host, clock) + def blocked(request): + entered.set() + assert release.wait(5) + return original(request) + monkeypatch.setattr(coordinator_module, "solve_initialization", blocked) + with ThreadPoolExecutor(1) as pool: + future = pool.submit(host.receive_detections, frame) + assert entered.wait(2) + # These calls need the real state lock while geometry is blocked. + clock.now += .001 + feedback = tuple(host.last_command) + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + assert host.state_history[-1].stamp_ns == host.ports.clock_ns() + host.snapshot() + # Solver wall time must not consume the separate two-second + # zero-image window. Feedback and the control timer keep running + # while the immutable preparation geometry is being solved. + for _ in range(6): + clock.now += .5 + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + host.tick() + assert host.execution.session.phase not in TERMINAL_PHASES, host.reason + assert host.reason.startswith("joint_zero_geometry_solving:") + assert not host.execution.zero_references + if change == "abort": + assert host.abort().success + else: + with host.step_data_lock: + host._segment_number += 1 + before = host.raw_path.read_bytes() + release.set() + future.result(timeout=5) + assert host.raw_path.read_bytes() == before + events = [json.loads(line) for line in + (host.parameters.session_dir / "motion_solver_diagnostic.jsonl").read_text().splitlines()] + assert [event["event"] for event in events] == ["request_created", "solve_finished", "commit_rejected"] + assert len({event["source_hashes_sha256"] for event in events}) == 1 + assert events[1]["elapsed_seconds"] >= 3.0 + assert not host.branch_initialization.reports + assert not host.execution.zero_references + assert not host.finalization.started + finally: + release.set() + host.close() + + +def test_late_previous_segment_image_does_not_consume_current_initialization(tmp_path, monkeypatch): + host, clock = start_diagnostic(tmp_path, monkeypatch) + try: + frame = before_initialization(host, clock) + monkeypatch.setattr(coordinator_module, "solve_initialization", + lambda request: pytest.fail("stale frame started initialization")) + before = host.raw_path.read_bytes() + host.receive_detections(DetectionInput("top", host._motion_started_ns-1, frame.detections)) + assert host.raw_path.read_bytes() == before + assert not host.branch_initialization._requested + # Existing image confirmation alone cannot bypass task geometry. + assert not host._lock_joint_zeros(host._motion) + assert host._zero_reference_error == "motion_geometry_confirmation_pending" + finally: + host.close() + + +def test_motion_geometry_confirmation_cannot_open_the_feedback_stability_gate(tmp_path, monkeypatch): + host, clock = start_diagnostic(tmp_path, monkeypatch) + try: + frame = before_initialization(host, clock) + assert host._steady_capture_after_ns is None + host.receive_detections(frame) + assert host.branch_initialization.ready(host._motion) + assert host._steady_capture_after_ns is None + assert not host._steady_rows + # Camera callbacks between control ticks cannot create a stable SDK + # window or contribute a zero, even with a verified geometric model. + for _ in range(15): + clock.now += .001 + host.receive_feedback(host.profile.command.names, host.last_command, host.ports.clock_ns()) + host.receive_detections(projected_frame(host, "top", host.last_command)) + assert not host._steady_rows + assert not host.execution.zero_references + assert not any(row["kind"] == "joint_zero_sample" for row in host.raw_records) + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_branch_worker.py b/src/linkerhand_calibration/test/test_branch_worker.py new file mode 100644 index 0000000..47087a0 --- /dev/null +++ b/src/linkerhand_calibration/test/test_branch_worker.py @@ -0,0 +1,49 @@ +"""Process transport preserves the pure solver and bounds failed workers.""" + +from dataclasses import replace +import multiprocessing +import json + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization +from linkerhand_calibration.runtime.branch_worker import IsolatedBranchSolver +from test_branch_initialization import arrival + + +def request(): + profile = load_bundled_hand_profile("o6_right_8") + approach, zero, rows = arrival(profile) + owner = BranchInitialization(profile, image_geometry=False) + owner.begin(approach, session_epoch=1, motion_version=7) + for row in rows: + owner.observe(row) + owner.begin(zero, session_epoch=1, motion_version=8) + return owner.request("top", zero, session_epoch=1, motion_version=8) + + +def test_process_returns_the_same_resolution_and_source_diagnostics(tmp_path): + item = request() + expected = solve_initialization(item) + path = tmp_path / "solver.jsonl" + worker = IsolatedBranchSolver(path) + try: + actual = worker(item) + assert actual == expected and actual.resolved + event = json.loads(path.read_text()) + assert event["event"] == "worker_solve_finished" + assert event["source_frames"] == len(item.frames) + assert event["thread_cpu_seconds"] > 0 + invalid = replace(item, invalid_reason="missing_motion_images") + assert worker(invalid).reason == "missing_motion_images" + finally: + worker.close() + assert not multiprocessing.active_children() + + +def test_timeout_and_closed_worker_never_leave_a_child_running(): + item = request() + worker = IsolatedBranchSolver(timeout_seconds=0.) + assert worker(item).reason == "image_motion_solver_timeout" + assert not multiprocessing.active_children() + worker.close() + assert worker(item).reason == "image_motion_solver_cancelled" diff --git a/src/linkerhand_calibration/test/test_cad_image_geometry.py b/src/linkerhand_calibration/test/test_cad_image_geometry.py new file mode 100644 index 0000000..2120210 --- /dev/null +++ b/src/linkerhand_calibration/test/test_cad_image_geometry.py @@ -0,0 +1,163 @@ +"""Source-invariant constraints, recorded ambiguity and independent image truth.""" + +from dataclasses import asdict, replace +import gzip +import hashlib +import json +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry +from linkerhand_calibration.core.geometry.tag_pose.cad_image_model import CadImageMotionModel +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates +from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( + resolve_image_motion, select_image_motion_frame, image_motion_model_from_dict, +) +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.profiles.observations import compile_parallel_axis_geometry +from linkerhand_calibration.runtime.motion_provenance import validate_source_geometry +from test_production_image_motion import recorded_image_frames + + +def product(name): + return load_product_config(Path(__file__).resolve().parents[1]/f"config/{name}_right_product.yaml", check_can=False) + + +@pytest.fixture(scope="module") +def recorded(): + raw=json.loads(gzip.decompress((Path(__file__).parent/"fixtures/o6_pinky_preparation_160040.json.gz").read_bytes())) + frames=recorded_image_frames(raw["frames"]) + relations=tuple(MotionRelation(**value) for value in raw["relations"]) + config=product("o6");profile=config.calibration_contract.typed_profile + constraints=compile_parallel_axis_geometry(profile,UrdfKinematicModel(config.source_urdf),relations) + result=resolve_image_motion(frames,relations,geometry_constraints=constraints, + source_urdf_sha256=raw["source_urdf_sha256"]) + return config,frames,relations,constraints,result + + +def test_recorded_ambiguity_resolves_with_unchanged_cad_and_original_gates(recorded): + config,frames,relations,constraints,result=recorded + assert len(frames)==104 + assert constraints[0].distance_m==pytest.approx(.03699959361344391,abs=1e-12) + assert hashlib.sha256(config.source_urdf.read_bytes()).hexdigest()==result.model.source_urdf_sha256 + free=resolve_image_motion(frames,relations) + assert not free.resolved and free.reason=="image_motion_families_not_distinguishable" + assert result.resolved,result.reason + assert len(result.hypotheses)==4 + assert all(h.converged and h.training_solver_evaluations<=150 for h in result.hypotheses) + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + assert result.model.maximum_reprojection_error_px==1.5 and result.model.reprojection_tie_px==.03 + assert image_motion_model_from_dict(asdict(result.model))==result.model + assert isinstance(result.model,CadImageMotionModel) + for frame in frames[1::8]: + measured=select_image_motion_frame(result.model,frame) + assert measured.resolved,measured.reason + assert max(q.maximum_frame_rms_px for q in measured.quality)<1.5 + + +@pytest.mark.parametrize("change", ["distance","axis","source","declared_distance"]) +def test_changed_geometry_or_source_cannot_become_frozen_evidence(recorded,change): + config,_,_,_,result=recorded + payload=json.loads(json.dumps(asdict(result.model))) + if change=="distance":payload["geometry"][1]["pivot_parent_xyz_m"][0]+=.01 + elif change=="axis":payload["geometry"][1]["axis_parent_xyz"]=payload["geometry"][0]["axis_parent_xyz"] + elif change=="declared_distance":payload["constraints"][0]["distance_m"]+=.01 + else:payload["source_urdf_sha256"]="f"*64 + if change!="source": + with pytest.raises(ValueError,match="source_axis"): + image_motion_model_from_dict(payload) + else: + from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 + record={"kind":"motion_branch_initialization","status":"resolved","frozen_image_model":payload, + "image_model_sha256":image_model_sha256(payload)} + with pytest.raises(ValueError,match="source_geometry_changed"): + validate_source_geometry(config.calibration_contract.typed_profile,config.source_urdf,[record]) + + +@pytest.mark.parametrize("name", ["g20","l6","o6","o12"]) +def test_compiled_constraints_remain_invariant_under_independent_joint_zeros(name): + config=product(name);profile=config.calibration_contract.typed_profile + model=UrdfKinematicModel(config.source_urdf) + for task in profile.motion.tasks: + relations=tuple(MotionRelation(joint,profile.measurement.measurements[joint].parent_role, + profile.measurement.measurements[joint].child_role) for joint in task.joints) + for constraint in compile_parallel_axis_geometry(profile,model,relations): + angles={joint:.11+.013*i for i,joint in enumerate(model.joints)} + transforms=[model.link_transform(joint,zero_offsets=angles,joint_angles=angles, + independent_mimic_angles=True) for joint in (constraint.parent_joint,constraint.child_joint)] + a,b=transforms + axis=a[:3,:3]@model.joints[constraint.parent_joint].axis + other=b[:3,:3]@model.joints[constraint.child_joint].axis + np.testing.assert_allclose(other,constraint.axis_sign*axis,atol=1e-8) + delta=b[:3,3]-a[:3,3] + assert np.linalg.norm(delta-axis*(axis@delta))==pytest.approx(constraint.distance_m,abs=1e-10) + + +def synthetic_frames(sign, hinges=2): + frames=[];truth=[] + camera=np.array([[2000.,0,800.],[0,2000.,600.],[0,0,1.]]) + root_r=Rotation.from_euler("xyz",(.2,-.15,.1));root_t=np.array([.0,.0,.8]) + mount_r=[Rotation.from_euler("xyz",(.13,.22,-.1)),Rotation.from_euler("xyz",(-.2,.1,.3))] + mount_t=[np.array([.01,.015,.004]),np.array([.012,-.017,.006])] + for i,q in enumerate(np.linspace(0,1.4,49)): + first_r=root_r*Rotation.from_rotvec([0,0,q]) + first_t=root_t+root_r.apply([.005,.012,.008]) + second_r=first_r*Rotation.from_rotvec([0,0,sign*(.6*q+.15*q*q)]) + second_t=first_t+first_r.apply([.037,0,.015]) + actual=[("root",root_r,root_t),("moving",first_r*mount_r[0],first_t+first_r.apply(mount_t[0])), + ("terminal",second_r*mount_r[1],second_t+second_r.apply(mount_t[1]))] + if hinges == 3: + third_r = second_r * Rotation.from_rotvec([0, 0, -sign * (.4*q + .12*q*q)]) + third_t = second_t + second_r.apply([.028, 0, -.005]) + actual.append(("tip", third_r * Rotation.from_euler("xyz", (.08, -.25, .19)), + third_t + third_r.apply([.012, .008, -.002]))) + observations=[];candidates=[];poses={} + for role,r,t in actual: + points=(r.apply(square_object_points(.016))+t)@camera.T + pixels=points[:,:2]/points[:,2:] + observations.append(ImageRoleObservation(role,.016,tuple(map(tuple,pixels)))) + poses[role]=SquareTagPose(tuple(r.as_quat()),tuple(t),0.) + noisy=t+([0,0,.003*np.sin(4*q)] if role!="root" else np.zeros(3)) + candidates.append(RoleCandidates(role,(SquareTagPose(tuple(r.as_quat()),tuple(noisy),.1),),role=="root")) + frames.append(ImageMotionFrame(MotionEvidenceFrame(1000000000+i*50000000,tuple(candidates)), + tuple(map(tuple,camera)),tuple(observations))) + truth.append(poses) + return tuple(frames),truth + + +@pytest.mark.parametrize("sign", [-1,1]) +@pytest.mark.parametrize("hinges", [2, 3]) +def test_independent_images_recover_known_motion_with_parallel_or_opposite_axes(sign, hinges): + frames,truth=synthetic_frames(sign, hinges) + relations=(MotionRelation("first","root","moving"),MotionRelation("second","moving","terminal")) + constraints=(ParallelAxisGeometry("first","second",sign,.037),) + if hinges == 3: + relations += (MotionRelation("third", "terminal", "tip"),) + constraints += (ParallelAxisGeometry("second", "third", -1, .028),) + result=resolve_image_motion(frames,relations,geometry_constraints=constraints, + source_urdf_sha256="a"*64) + assert result.resolved,result.reason + for i in range(1,len(frames),8): + selected=select_image_motion_frame(result.model,frames[i]) + assert selected.resolved,selected.reason + for value in selected.selections: + actual=truth[i][value.role] + np.testing.assert_allclose(value.pose.translation_xyz_m,actual.translation_xyz_m,atol=1e-6) + assert (Rotation.from_quat(actual.quaternion_xyzw).inv()*Rotation.from_quat(value.pose.quaternion_xyzw)).magnitude()<1e-5 + + +def test_geometry_incompatible_with_images_cannot_be_forced_through_acceptance(): + frames, _ = synthetic_frames(1) + relations = (MotionRelation("first", "root", "moving"), MotionRelation("second", "moving", "terminal")) + result = resolve_image_motion(frames, relations, + geometry_constraints=(ParallelAxisGeometry("first", "second", 1, .08),), + source_urdf_sha256="a"*64) + assert not result.resolved + assert all(hypothesis.reason == "image_motion_reprojection_failed" for hypothesis in result.hypotheses) diff --git a/src/linkerhand_calibration/test/test_calibrated_motion_limits.py b/src/linkerhand_calibration/test/test_calibrated_motion_limits.py new file mode 100644 index 0000000..2e17bc7 --- /dev/null +++ b/src/linkerhand_calibration/test/test_calibrated_motion_limits.py @@ -0,0 +1,161 @@ +"""Measured amplitude may replace nominal CAD limits, never safety evidence.""" + +from dataclasses import replace +import hashlib +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.core.domain.profile import JointLimitPolicy, validate_profile +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.acceptance import validate_artifact_structure +from linkerhand_calibration.core.urdf.limits import MotionRangeEvidence, compile_motion_ranges, validate_range_evidence +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.export import write_hand_profile +from linkerhand_calibration.profiles.loader import load_hand_profile + + +SOURCE = Path(__file__).resolve().parents[1] / "urdf/o6_right/linkerhand_o6_right.urdf" +PITCH, IP = "rh_thumb_cmc_pitch", "rh_thumb_ip" + + +def evidence(name, upper, *, delta=0., safety=None): + original = UrdfKinematicModel(SOURCE).joints[name] + return MotionRangeEvidence(name, name, True, (original.lower, original.upper), + (0., upper), (0., upper), (0., upper), delta, 255., (0, 1, 2), + ("front:100", "front:200"), ("front:10",), "calibrated_motion_envelope", + "fixture nominal CAD amplitude policy", safety, "fixture independent hardware bound" if safety else "") + + +def plan(evidence_by_joint=None, *, authorized=None): + profile = load_bundled_hand_profile("o6_right_8") + return build_standard_correction_plan(source_urdf=SOURCE, + source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(), + zero_offsets_rad={PITCH: 0., IP: 0.}, + measured_ranges_output_rad={PITCH: (0., .60), IP: (0., 1.16)}, + range_evidence=evidence_by_joint, + authorized_fields=profile.urdf_authorized_fields if authorized is None else authorized) + + +def test_nominal_range_requires_explicit_training_evidence_and_authorization(tmp_path): + with pytest.raises(ValueError, match="exceeds source mechanical range"): + plan() + entries = {PITCH: evidence(PITCH, .60), IP: evidence(IP, 1.16)} + original = SOURCE.read_bytes() + result = plan(entries) + output = tmp_path / "corrected.urdf" + result.write(SOURCE, output) + before, after = UrdfKinematicModel(SOURCE), UrdfKinematicModel(output) + assert SOURCE.read_bytes() == original + assert after.joints[IP].upper == 1.16 + assert (after.joints[IP].origin == before.joints[IP].origin).all() + assert result.range_evidence[IP].training_cycles == (0, 1, 2) + rights = dict(load_bundled_hand_profile("o6_right_8").urdf_authorized_fields) + rights[IP] = frozenset({"mimic.multiplier", "mimic.offset"}) + with pytest.raises(ValueError, match="absent from Profile authorization"): + plan(entries, authorized=rights) + + +@pytest.mark.parametrize("mutation,reason", [ + ({"training_cycles": (0, 1, 2, 3)}, "invalid_training"), + ({"training_sample_ids": ()}, "invalid_training"), + ({"reference_sample_ids": ()}, "invalid_training"), + ({"output_limits_rad": (0., 1.17)}, "differs_from_training"), + ({"independent_safety_limits_cad_rad": (0., 1.1), "safety_evidence_source": "known hardware"}, "independent_safety"), + ({"independent_safety_limits_cad_rad": (0., 2.), "safety_evidence_source": ""}, "independent_safety"), +]) +def test_evidence_cannot_use_holdout_pad_or_override_independent_safety(mutation, reason): + item = replace(evidence(IP, 1.16), **mutation) + with pytest.raises(ValueError, match=reason): + validate_range_evidence(IP, UrdfKinematicModel(SOURCE).joints[IP], 0., (0., 1.16), item) + + +def test_independent_safety_is_checked_in_cad_coordinates_once(): + original = UrdfKinematicModel(SOURCE).joints[PITCH] + item = evidence(PITCH, .6, delta=.1, safety=(.09, .71)) + validate_range_evidence(PITCH, original, .1, (0., .6), item) + with pytest.raises(ValueError, match="independent_safety"): + validate_range_evidence(PITCH, original, .1, (0., .6), + replace(item, independent_safety_limits_cad_rad=(.09, .69))) + + +def test_final_readback_needs_trusted_exact_envelope_not_payload_permission(tmp_path): + import xml.etree.ElementTree as ET + entries = {PITCH: evidence(PITCH, .60), IP: evidence(IP, 1.16)} + result = plan(entries) + output = tmp_path / "corrected.urdf" + result.write(SOURCE, output) + profile = load_bundled_hand_profile("o6_right_8") + from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint + model = UrdfKinematicModel(output) + payload = {"format": "unified_calibration_v3", "schema_version": 3, + "input_unit": "u8", "baseline_command": [255]*6, + "joints": {name: {"sdk_channel": channel_for_joint(profile, name), + "angle_rad": [joint.upper*(255-code)/255 for code in range(256)]} + for name, joint in model.joints.items() if joint.kind != "fixed"}} + args = dict(source_urdf=SOURCE, corrected_urdf=output, + source_sha256=result.source_sha256, authorized_fields=profile.urdf_authorized_fields, + payload=payload, zero_offsets_rad={PITCH: 0., IP: 0.}) + assert validate_artifact_structure(**args, range_evidence=entries) + with pytest.raises(ValueError, match="exceeds source mechanical range"): + validate_artifact_structure(**args) + tree = ET.parse(output) + tree.find(f".//joint[@name='{PITCH}']/limit").set("upper", ".601") + tree.write(output) + with pytest.raises(ValueError, match="differs_from_training"): + validate_artifact_structure(**args, range_evidence=entries) + + +def test_mimic_does_not_expand_measured_envelope_to_make_it_pass(): + with pytest.raises(ValueError, match="differs_from_training"): + plan({PITCH: evidence(PITCH, .6), IP: evidence(IP, 1.09)}) + # A validly supported 1.09 rad child cannot accommodate the unchanged + # standard mimic's 1.86 * .6 range; the plan must reject, never add padding. + profile = load_bundled_hand_profile("o6_right_8") + with pytest.raises(ValueError, match="mimic reachable range exceeds"): + build_standard_correction_plan(source_urdf=SOURCE, + source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(), + zero_offsets_rad={PITCH: 0., IP: 0.}, + measured_ranges_output_rad={PITCH: (0., .6), IP: (0., 1.09)}, + range_evidence={PITCH: evidence(PITCH, .6), IP: evidence(IP, 1.09)}, + authorized_fields=profile.urdf_authorized_fields) + + +def test_profile_roundtrip_keeps_explicit_policy_and_dip_origin_protection(tmp_path): + profile = load_bundled_hand_profile("o6_right_8") + output = tmp_path / "profile.yaml" + write_hand_profile(profile, output) + assert load_hand_profile(output) == profile + assert len(profile.urdf_limit_policies) == 11 + assert all("origin.rpy" not in profile.urdf_authorized_fields[name] for name in profile.zero.passive_joints) + policies = dict(profile.urdf_limit_policies) + policies[IP] = JointLimitPolicy("calibrated_motion_envelope", "source policy", (0., 2.), "") + with pytest.raises(ValueError, match="safety bounds"): + validate_profile(replace(profile, urdf_limit_policies=policies)) + + +def test_transferred_envelope_keeps_donor_training_and_ignores_holdout_errors(): + donor, target = "rh_pinky_dip", "rh_index_dip" + profile = load_bundled_hand_profile("o6_right_8") + profile = replace(profile, urdf_limit_policies={target: profile.urdf_limit_policies[target]}) + def mapping(domain): + return JointMapping(target, 2, domain, (0., 255.), (1.5, 0.), (1.51, 0.), (1.49, 0.), donor) + fit = SimpleNamespace(curves={donor: SimpleNamespace(circle={"training_cycles": [0, 1, 2], + "training_sample_ids": ["side:100", "side:200"]})}, + joint_zero_references={donor: SimpleNamespace(baseline_command=255., + samples=[SimpleNamespace(view="side", image_stamp_ns=10)])}, + command_holdout_metrics={donor: {"training_cycles": [4], "holdout_cycle": 5, + "training_sample_ids": ["side:300", "side:400"], "maximum_deg": 2}}, + output_mappings={target: mapping("feedback_u8")}, command_mappings={target: mapping("command_u8")}, + zero_offsets_rad={target: 0.}) + model = UrdfKinematicModel(SOURCE) + before = compile_motion_ranges(profile, model, fit)[target] + fit.command_holdout_metrics[donor]["maximum_deg"] = 100 + after = compile_motion_ranges(profile, model, fit)[target] + assert before == after + assert before.source_joint == donor and before.independently_measured is False + assert before.output_limits_rad == (0., 1.51) + assert before.training_sample_ids == ("side:100", "side:200") diff --git a/src/linkerhand_calibration/test/test_calibration_coordinator.py b/src/linkerhand_calibration/test/test_calibration_coordinator.py index 3ba935f..2b0f0e0 100644 --- a/src/linkerhand_calibration/test/test_calibration_coordinator.py +++ b/src/linkerhand_calibration/test/test_calibration_coordinator.py @@ -2,6 +2,7 @@ from concurrent.futures import ThreadPoolExecutor from dataclasses import replace +import json from threading import Event from types import SimpleNamespace @@ -15,7 +16,7 @@ from linkerhand_calibration.runtime.session import CalibrationPhase as Phase from runtime_host_fixture import coordinator_fixture, ready -@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady", "joint_zero"]) +@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady", "joint_zero", "fit"]) def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monkeypatch, change): host, clock = coordinator_fixture(tmp_path) entered, release = Event(), Event() @@ -49,12 +50,17 @@ def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monk elif change == "motion": with host.step_data_lock: host._motion = replace(host._motion, cycle=1) + elif change == "fit": + monkeypatch.setattr(host.finalization, "start", lambda _: None) + with host.step_data_lock: + host.execution.session.phase = Phase.FIT + host._finalize() else: with host.step_data_lock: host._steady_capture_after_ns = None release.set() future.result(timeout=2) - assert host.raw_records == [] + assert [row["kind"] for row in host.raw_records] == ["session_start"] assert host._unit_rows == [] finally: release.set() @@ -85,8 +91,9 @@ def replace_finalizer(host, finalizer): host.finalization = FinalizationController(host.profile, host.parameters.session_dir, finalizer=finalizer) -def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp_path): - host, clock = coordinator_fixture(tmp_path) +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp_path, model): + host, clock = coordinator_fixture(tmp_path, model=model) entered, release = Event(), Event() def fit(**kwargs): entered.set() @@ -106,8 +113,22 @@ def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp with pytest.raises(ValueError, match="synthetic_fit_failure"): host.finalization.worker._future.result(timeout=2) host.tick() - assert host.execution.session.phase == Phase.PAUSED + assert host.execution.session.phase == Phase.FAILED assert "fit_or_publication_failed:synthetic_fit_failure" == host.reason + diagnostic = json.loads((host.parameters.session_dir / "failure_diagnostic.json").read_text()) + assert diagnostic["stage"] == "FIT" + assert diagnostic["raw_samples"] == str(host.raw_path) + assert diagnostic["reason"] == "synthetic_fit_failure" + assert 'ValueError: synthetic_fit_failure' in diagnostic["traceback"] + rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()] + assert rows[-1]["kind"] == "finalization_failed" + assert not any(row["kind"] == "paused" for row in rows) + assert host.snapshot().calibration.outputs["failure_diagnostic"].endswith("failure_diagnostic.json") + assert not host.final_json and not host.final_urdf + # Terminal failure must survive late sensor events and operator cleanup. + host._pause("late_observation_failure") + assert not host.abort().success + assert host.execution.session.phase == Phase.FAILED count = len(clock.positions) host.tick() assert len(clock.positions) == count @@ -117,6 +138,56 @@ def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp host.close() +def test_finalization_protocol_failure_is_durable_and_cannot_publish(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + commits = [] + def invalid_order(**kwargs): + kwargs["phase_changed"]("urdf_validated") + return ({}, None, SimpleNamespace(staged_release="must_not_commit")) + try: + replace_finalizer(host, invalid_order) + monkeypatch.setattr(ArtifactPublisher, "commit", lambda _, staged: commits.append(staged)) + begin_fit(host, clock) + host.finalization.worker._future.result(timeout=2) + host.tick() + assert host.execution.session.phase == Phase.FAILED + assert host.reason.startswith("finalization_protocol_error:") + assert not commits + diagnostic = json.loads((host.parameters.session_dir / "failure_diagnostic.json").read_text()) + assert diagnostic["state"] == "FAILED" and diagnostic["stage"] == "FIT" + finally: + host.close() + + +def test_fit_uses_closed_capture_while_feedback_and_abort_remain_live(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + entered, release = Event(), Event() + def fit(**kwargs): + entered.set() + assert release.wait(2) + return completed_fit(**kwargs) + try: + replace_finalizer(host, fit) + begin_fit(host, clock) + assert entered.wait(2) + before = host.raw_path.read_bytes() + view = host.required_observation_views[0] + monkeypatch.setattr(host.capture, "consume", lambda *a, **k: pytest.fail("capture is closed")) + monkeypatch.setattr(host.cameras, "accept", lambda *a, **k: pytest.fail("camera evidence is frozen")) + host.receive_detections(DetectionInput(view, host.ports.clock_ns(), ())) + host.receive_camera_info(view, None) + assert host.raw_path.read_bytes() == before + assert host.execution.session.phase == Phase.FIT + clock.now += .02 + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + assert host.state_history[-1].stamp_ns == host.ports.clock_ns() + assert host.abort().success + assert host.execution.session.phase == Phase.ABORTED + finally: + release.set() + host.close() + + @pytest.mark.parametrize("abort_first", [True, False]) def test_abort_and_artifact_commit_are_mutually_exclusive(tmp_path, monkeypatch, abort_first): host, clock = coordinator_fixture(tmp_path) diff --git a/src/linkerhand_calibration/test/test_camera_timing.py b/src/linkerhand_calibration/test/test_camera_timing.py new file mode 100644 index 0000000..bd2ac72 --- /dev/null +++ b/src/linkerhand_calibration/test/test_camera_timing.py @@ -0,0 +1,93 @@ +"""Device clocks, transport delay and exposure centre are separate quantities.""" + +import gzip +import json +from pathlib import Path +import statistics + +import pytest + +from linkerhand_calibration.camera_timing import ClockLatch, DeviceFrameClock, MvsCameraTiming + + +T0 = 1_789_476_000_000_000_000 + + +def latch(elapsed_ns, *, width=200_000, host_jump=0): + return ClockLatch(10_000_000_000+elapsed_ns//10, + T0+elapsed_ns-width//2+host_jump, T0+elapsed_ns+width//2+host_jump) + + +def initialized(): + result = DeviceFrameClock(100_000_000) + result.update([latch(0),latch(100_000_000)]) + return result + + +def test_transport_delay_does_not_change_exposure_timestamp(): + for delivery_ns in (16_000_000, 120_000_000): + clock = initialized() + stamp = clock.timestamp(latch(200_000_000).device_tick, + received_ns=T0+200_000_000+delivery_ns, exposure_us=5000.) + assert stamp == T0+202_500_000 + + +def test_counter_unit_mismatch_and_clock_jump_cannot_forge_new_timestamps(): + wrong = DeviceFrameClock(1_000_000_000) + with pytest.raises(ValueError, match='unit mismatch'): + wrong.update([latch(0),latch(100_000_000)]) + clock = initialized() + anchor = clock.anchor + with pytest.raises(ValueError, match='discontinuity'): + clock.update([latch(500_000_000,host_jump=10_000_000)]) + assert clock.anchor == anchor + + +def test_frame_reset_duplicates_future_and_stale_clocks_are_rejected(): + clock = initialized() + tick = latch(200_000_000).device_tick + clock.timestamp(tick,received_ns=T0+220_000_000,exposure_us=5000.) + for duplicate in (tick,tick-1): + with pytest.raises(ValueError,match='reset or duplicate'): + clock.timestamp(duplicate,received_ns=T0+240_000_000,exposure_us=5000.) + with pytest.raises(ValueError,match='future'): + clock.timestamp(latch(900_000_000).device_tick,received_ns=T0+240_000_000,exposure_us=5000.) + with pytest.raises(ValueError,match='fresh clock'): + clock.timestamp(latch(2_500_000_000).device_tick,received_ns=T0+2_520_000_000,exposure_us=5000.) + + +def test_uncertain_control_transaction_does_not_refresh_the_clock(): + timing = MvsCameraTiming(None,None,lambda:0,exposure_us=5000.) + timing.frame_clock = initialized() + anchor = timing.frame_clock.anchor + timing._update([latch(500_000_000,width=20_000_000)]) + assert timing.frame_clock.anchor == anchor + timing._update([latch(600_000_000)]) + assert timing.frame_clock.anchor == latch(600_000_000) + + +@pytest.mark.parametrize('serial', ['DB2163742','DB2163749','DB2163739']) +def test_real_camera_counter_units_and_exposure_events(serial): + fixture = Path(__file__).parent/'fixtures/camera_clock_receipts.json.gz' + data = json.loads(gzip.decompress(fixture.read_bytes()))[serial] + assert all(all(status == 0 for status in item['statuses']) for item in data['events']['setup']) + latches = [ClockLatch(x['nodes']['DeviceTimestamp']['value'],x['before_ns'],x['read_return_ns']) + for x in data['latches']] + timing = MvsCameraTiming(None,None,lambda:0,exposure_us=5000.) + timing.frame_clock = DeviceFrameClock(data['latches'][0]['nodes']['DeviceTimestampIncrement']['value']) + timing._update(latches[:5]) + clock = timing.frame_clock + exposure_events = [x['tick'] for x in data['events']['events'] if x['event']=='ExposureStart'] + delays, updates = [], 5 + for frame in data['frames']: + if frame['host_unix_ns'] <= latches[4].after_ns: + continue + if updates < len(latches) and latches[updates].after_ns <= frame['host_unix_ns']: + timing._update([latches[updates]]) + updates += 1 + stamp = clock.timestamp(frame['camera_tick'],received_ns=frame['host_unix_ns'],exposure_us=5000.) + delays.append((frame['host_unix_ns']-stamp)/1e6) + if exposure_events[0] <= frame['camera_tick'] <= exposure_events[-1]: + assert min(abs(tick-frame['camera_tick']) for tick in exposure_events)*10 <= 200 + assert len(delays)>40 + assert 12 < statistics.median(delays) < 16 diff --git a/src/linkerhand_calibration/test/test_capture_journal.py b/src/linkerhand_calibration/test/test_capture_journal.py new file mode 100644 index 0000000..64a753e --- /dev/null +++ b/src/linkerhand_calibration/test/test_capture_journal.py @@ -0,0 +1,85 @@ +"""Continuous observation logging must not fsync under the control lock.""" + +from concurrent.futures import ThreadPoolExecutor +from threading import Event + +import pytest + +from linkerhand_calibration.core.artifacts import storage +from linkerhand_calibration.runtime.inputs import DetectionInput +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture +from test_runtime_pose_branch_guard import begin_yaw, branch_event + + +def test_sweep_records_do_not_wait_for_disk_sync_and_pause_persists_them(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + release, entered = Event(), Event() + try: + begin_yaw(host, clock) + stamp = host.ports.clock_ns() + rows = [branch_event(stamp, reason="pose_branch_ambiguous"), + {"kind": "diagnostic_observation_frame", "view": "top", "image_stamp_ns": stamp}] + monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: (rows, None)) + def slow_sync(_fd): + entered.set() + assert release.wait(3) + with monkeypatch.context() as patch: + patch.setattr(storage.os, "fsync", slow_sync) + with ThreadPoolExecutor(2) as pool: + try: + pool.submit(host.receive_detections, DetectionInput("top", stamp, ())).result(timeout=1) + clock.now += .04 + pool.submit(host.receive_feedback, host.command_names, + host.profile.command.baseline_values, host.ports.clock_ns()).result(timeout=1) + assert not entered.is_set() + assert host.state_history[-1].stamp_ns == host.ports.clock_ns() + finally: + release.set() + assert host.raw_records[0] == storage.load_jsonl(host.raw_path)[0] + assert host.raw_records[1:] == rows + assert storage.load_jsonl(host.raw_path)[-2:] == rows + persisted = [] + with monkeypatch.context() as patch: + patch.setattr(storage.os, "fsync", lambda _fd: persisted.append(storage.load_jsonl(host.raw_path))) + host._pause("test_pause") + assert persisted and persisted[-1][-1]["kind"] == "paused" + assert persisted[-1][-3:-1] == rows + finally: + release.set() + host.close() + + +@pytest.mark.parametrize("boundary", ["fit", "commit", "diagnostic"]) +def test_sync_failure_blocks_fitting_publication_and_diagnostic_success(tmp_path, monkeypatch, boundary): + from linkerhand_calibration.runtime import coordinator + host, _ = coordinator_fixture(tmp_path, + diagnostic_capture="o6_thumb" if boundary == "diagnostic" else "") + def failed(_path): + raise OSError("simulated journal sync failure") + try: + with monkeypatch.context() as patch: + patch.setattr(coordinator, "sync_jsonl", failed) + if boundary == "diagnostic": + host.execution.session.phase = Phase.DIAGNOSTIC_COMPLETE + host._finish_diagnostic_capture() + assert host.execution.session.phase == Phase.FAILED + assert not (host.parameters.session_dir / "diagnostic_capture.json").exists() + else: + host.execution.session.phase = Phase.FIT if boundary == "fit" else Phase.PUBLISH + with pytest.raises(OSError, match="journal sync failure"): + (host._finalize if boundary == "fit" else host._authorize_commit)() + assert not host.finalization.started + assert not host.final_json and not host.final_urdf + finally: + host.close() + + +def test_close_syncs_last_observation_records(tmp_path, monkeypatch): + from linkerhand_calibration.runtime import coordinator + host, _ = coordinator_fixture(tmp_path) + epoch = host._observation_epoch + calls = [] + monkeypatch.setattr(coordinator, "sync_jsonl", lambda path: calls.append((path, host._observation_epoch))) + host.close() + assert calls == [(host.raw_path, epoch + 1)] diff --git a/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py b/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py new file mode 100644 index 0000000..f87795c --- /dev/null +++ b/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py @@ -0,0 +1,111 @@ +"""Pose-cache snapshots stay coherent while camera callbacks keep running.""" + +from concurrent.futures import ThreadPoolExecutor +from threading import Event + +from test_observation_capture import frame, make_capture + + +def test_fingerprint_snapshots_dictionary_and_deques_before_computing_means(monkeypatch): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(1, 16): + capture.consume(frame(profile, stamp, omit=("thumb_dip",)), collect_installations=True) + expected = capture.fingerprint_poses() + original_count = capture.installation_count("front:thumb_pitch") + entered, release = Event(), Event() + original_mean = capture._median_pose + snapshots = [] + def mean(poses): + snapshots.append(poses) + entered.set() + assert release.wait(2) + return original_mean(poses) + monkeypatch.setattr(capture, "_median_pose", mean) + try: + with ThreadPoolExecutor(1) as pool: + future = pool.submit(capture.fingerprint_poses) + assert entered.wait(2) + # Add another role and mutate the existing deque while its old + # fingerprint's expensive mean is intentionally blocked. + for stamp in range(20, 25): + capture.consume(frame(profile, stamp), collect_installations=True) + assert capture.installation_count("front:thumb_dip") > 0 + assert capture.installation_count("front:thumb_pitch") > original_count + assert capture.locked_reference_views() == {"front"} + assert isinstance(snapshots[0], tuple) + assert len(snapshots[0]) == original_count + release.set() + assert future.result(timeout=2) == expected + finally: + release.set() + + +def test_fixed_pose_mean_never_holds_the_metadata_read_lock(monkeypatch): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(1, 14): + capture.consume(frame(profile, stamp)) + assert not capture.locked_reference_views() + entered, release = Event(), Event() + original_mean = capture._median_pose + def mean(poses): + assert isinstance(poses, tuple) + entered.set() + assert release.wait(2) + return original_mean(poses) + monkeypatch.setattr(capture, "_median_pose", mean) + try: + with ThreadPoolExecutor(1) as pool: + future = pool.submit(capture.consume, frame(profile, 14)) + assert entered.wait(2) + assert capture.fingerprint_poses() == ({}, {}) + assert capture.installation_count("front:thumb_pitch") == 0 + assert capture.locked_reference("front") is None + release.set() + future.result(timeout=2) + assert capture.locked_reference_views() == {"front"} + finally: + release.set() + + +def test_old_shifted_images_cannot_trigger_false_fixed_reference_drift(): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(100, 115): + capture.consume(frame(profile, stamp)) + before_stamps = capture._latest_stamp_by_view.copy() + before_fingerprint = capture.fingerprint_poses() + for stamp in range(1, 11): + assert capture.consume(frame(profile, stamp, shift=6)) == ((), None) + assert capture._latest_stamp_by_view == before_stamps + assert capture.fingerprint_poses() == before_fingerprint + assert lock.latest_drift_px["front"] == 0. + assert capture.trackers["front"].last_candidate_diagnostics_by_role["front_base"]["observation_stamp_ns"] == 114*40_000_000 + + +def test_old_normal_images_cannot_clear_a_real_fixed_reference_drift(): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(100, 115): + capture.consume(frame(profile, stamp)) + for stamp in range(120, 129): + assert capture.consume(frame(profile, stamp, shift=6))[1] is None + for stamp in range(1, 11): + assert capture.consume(frame(profile, stamp)) == ((), None) + movement = capture.consume(frame(profile, 129, shift=6))[1] + assert movement is not None + assert movement.confirmation_frames == 10 + assert movement.drift_px == 6. + + +def test_capture_reset_resets_each_view_timestamp_gate(): + profile, lock, capture = make_capture() + capture.consume(frame(profile, 100)) + capture.reset() + capture.consume(frame(profile, 1)) + assert capture._latest_stamp_by_view == {"front": 40_000_000} diff --git a/src/linkerhand_calibration/test/test_capture_provenance.py b/src/linkerhand_calibration/test/test_capture_provenance.py index 152480d..0b575cb 100644 --- a/src/linkerhand_calibration/test/test_capture_provenance.py +++ b/src/linkerhand_calibration/test/test_capture_provenance.py @@ -1,23 +1,29 @@ """Old/edited journals cannot silently acquire current configuration hashes.""" import copy +from dataclasses import replace import hashlib import json import pytest +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION from linkerhand_calibration.profiles import load_bundled_hand_profile from linkerhand_calibration.runtime.artifacts.replay import validate_capture_provenance, validate_capture_units -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION, CalibrationEngine from linkerhand_calibration.runtime.scan_quality import observation_streams def journal(): profile = load_bundled_hand_profile("l6_right_8") + # This fixture predates an explicit schedule header and models historical + # interleaved capture, not today's separate production schedule. + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved")) hashes = {"source_urdf_sha256": "a"*64, "profile_config_sha256": "b"*64} matrices = {view: [[100., 0., 50.], [0., 100., 50.], [0., 0., 1.]] for view in profile.vision.view_names} header = {"kind": "session_start", "profile_id": profile.key.profile_id, "serial_number": "VIRTUAL", "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, "curve_input_domain": profile.curve_input_domain, **hashes} reference = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, @@ -31,12 +37,14 @@ def journal(): return profile, hashes, rows -@pytest.mark.parametrize("mutation", ["old_policy", "changed_hash", "missing_reference", "projection", "input_domain"]) +@pytest.mark.parametrize("mutation", ["old_policy", "old_pose_policy", "changed_hash", "missing_reference", "projection", "input_domain"]) def test_changed_or_missing_acquisition_provenance_cannot_publish(mutation): profile, hashes, rows = journal() validate_capture_provenance(profile, "VIRTUAL", hashes, rows) if mutation == "old_policy": rows[0]["acquisition_policy_version"] = "unified_engine_v2" + elif mutation == "old_pose_policy": + rows[0].pop("pose_tracking_policy_version") elif mutation == "changed_hash": rows[0]["source_urdf_sha256"] = "c"*64 elif mutation == "missing_reference": @@ -49,6 +57,26 @@ def test_changed_or_missing_acquisition_provenance_cannot_publish(mutation): validate_capture_provenance(profile, "VIRTUAL", hashes, rows) +def test_historical_capture_can_be_checked_without_mixing_it_into_new_motion(): + profile, hashes, rows = journal() + before = copy.deepcopy(rows) + validate_capture_provenance(profile, "VIRTUAL", hashes, rows) + assert rows == before + assert not CalibrationEngine(profile).resume_compatible(rows[0]) + current = copy.deepcopy(rows) + current[0]["capture_schedule_version"] = CAPTURE_SCHEDULE_VERSION + validate_capture_provenance(profile, "VIRTUAL", hashes, current) + assert CalibrationEngine(profile).resume_compatible(current[0]) + + +@pytest.mark.parametrize("schedule", ["unknown_future_schedule", {}, []]) +def test_unknown_capture_schedule_cannot_publish(schedule): + profile, hashes, rows = journal() + rows[0]["capture_schedule_version"] = schedule + with pytest.raises(ValueError, match="capture_provenance"): + validate_capture_provenance(profile, "VIRTUAL", hashes, rows) + + def test_offline_units_use_live_quality_and_require_completed_last_attempt(): profile, _, rows = journal() stamp = 0 diff --git a/src/linkerhand_calibration/test/test_detection_dispatch.py b/src/linkerhand_calibration/test/test_detection_dispatch.py new file mode 100644 index 0000000..828accc --- /dev/null +++ b/src/linkerhand_calibration/test/test_detection_dispatch.py @@ -0,0 +1,109 @@ +"""Image lookahead cannot monopolize feedback callbacks or grow a backlog.""" + +from threading import Event +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.runtime.detection_dispatch import DetectionDispatcher + + +def observation(stamp, view='front'): + return SimpleNamespace(view=view, stamp_ns=stamp) + + +def test_waits_for_feedback_to_cover_image_before_delivering(): + delivered = Event() + dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(), + maximum_wait_seconds=1., failed=lambda error: None) + try: + dispatcher.submit(observation(20)) + dispatcher.feedback_available(19) + assert not delivered.wait(.02) + dispatcher.feedback_available(21) + assert delivered.wait(.2) + finally: + dispatcher.close() + + +def test_slow_image_work_retains_only_latest_pending_frame_per_view(): + running, release, finished = Event(), Event(), Event() + images = [] + def deliver(row): + images.append(row.stamp_ns) + if row.stamp_ns == 1: + running.set() + assert release.wait(1.) + else: + finished.set() + dispatcher = DetectionDispatcher(('front',), deliver, + maximum_wait_seconds=.05, failed=lambda error: None) + try: + dispatcher.feedback_available(100) + dispatcher.submit(observation(1)) + assert running.wait(1.) + for stamp in range(2, 100): + dispatcher.submit(observation(stamp)) + # Feedback and submissions cannot wait for blocked geometry or the + # coordinator lock held by that geometry's consumer. + dispatcher.feedback_available(101) + release.set() + assert finished.wait(1.) + assert images == [1, 99] + finally: + release.set() + dispatcher.close() + + +def test_start_or_abort_discards_images_waiting_for_feedback(): + delivered = Event() + dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(), + maximum_wait_seconds=1., failed=lambda error: None) + dispatcher.submit(observation(10)) + assert not delivered.wait(.02) + dispatcher.clear() + dispatcher.feedback_available(11) + dispatcher.close() + assert not delivered.is_set() + + +def test_new_session_cannot_use_the_previous_sessions_feedback_watermark(): + delivered = Event() + dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(), + maximum_wait_seconds=1., failed=lambda error: None) + try: + dispatcher.feedback_available(100) + dispatcher.clear() + dispatcher.submit(observation(20)) + assert not delivered.wait(.02) + dispatcher.feedback_available(21) + assert delivered.wait(.2) + finally: + dispatcher.close() + + +def test_missing_feedback_has_a_bounded_wait_and_capture_still_decides_validity(): + delivered = Event() + dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(), + maximum_wait_seconds=.02, failed=lambda error: None) + try: + dispatcher.submit(observation(10)) + assert delivered.wait(.5) + finally: + dispatcher.close() + + +def test_worker_errors_reach_the_control_thread(): + failed = Event() + def fail(_): + raise ValueError('unexpected geometry error') + dispatcher = DetectionDispatcher(('front',), fail, + maximum_wait_seconds=.02, failed=lambda error: failed.set()) + try: + dispatcher.submit(observation(10)) + assert failed.wait(1.) + with pytest.raises(RuntimeError, match='image dispatch failed') as error: + dispatcher.raise_if_failed() + assert isinstance(error.value.__cause__, ValueError) + finally: + dispatcher.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_analysis.py b/src/linkerhand_calibration/test/test_diagnostic_analysis.py new file mode 100644 index 0000000..fd640d8 --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_analysis.py @@ -0,0 +1,210 @@ +"""Detached analysis boundaries and source integrity; expensive fitting is mocked.""" + +from copy import deepcopy +import hashlib +import json + +import pytest + +from linkerhand_calibration.core.geometry.tag_pose import motion_image_diagnostics +from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import MotionImageDiagnosticReport +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime import diagnostic_analysis as analysis +from test_recorded_diagnostic_replay import recorded_fixture + + +def write_actual_capture(directory): + fixture = recorded_fixture("192637") + records = [fixture["session_start"], fixture["camera_model"], *fixture["records"], + fixture["recorded_initialization"]] + path = directory / "raw_samples.jsonl" + path.write_text("".join(json.dumps(row)+"\n" for row in records)) + return fixture, records, path + + +def mock_image_analysis(monkeypatch, callback=None): + calls = [] + def solve(frames, relations): + calls.append((frames, relations)) + if callback is not None: + callback() + return MotionImageDiagnosticReport("complete", "", tuple(frame.evidence.stamp_ns for frame in frames)) + monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images", solve) + return calls + + +def test_actual_initialization_images_remain_bound_in_separate_analysis_output(tmp_path, monkeypatch): + fixture, _, raw = write_actual_capture(tmp_path) + before = raw.read_bytes() + (tmp_path / "diagnostic_capture.json").write_text('{"original": true}') + original = (tmp_path / "diagnostic_capture.json").read_bytes() + calls = mock_image_analysis(monkeypatch) + profile = load_bundled_hand_profile("o6_right_8") + output = tmp_path / "analysis" + result = analysis.analyze_capture(profile, raw, output_dir=output) + assert len(calls) == 1 + frames, relations = calls[0] + assert len(frames) == 120 + assert [frame.evidence.stamp_ns for frame in frames] == fixture["recorded_initialization"]["source_image_stamps"] + assert {relation.joint for relation in relations} == {"rh_thumb_cmc_pitch", "rh_thumb_ip"} + assert result["raw_samples_sha256"] == hashlib.sha256(before).hexdigest() + assert result["publication_allowed"] is result["accuracy_validated"] is False + assert result["moving_candidates_counterfactual"] is True + assert result["stages"][0]["recorded_branch_status"] == "unresolved" + assert result["stages"][0]["source_image_count"] == 120 + assert json.loads((output / "diagnostic_analysis.json").read_text())["publication_allowed"] is False + assert (output / "diagnostic_analysis.md").exists() + assert raw.read_bytes() == before + assert (tmp_path / "diagnostic_capture.json").read_bytes() == original + assert not list(tmp_path.rglob("*.urdf")) + + +@pytest.mark.parametrize("fault", ["second_session", "profile", "image_content", "source_identity", "duplicate_image"]) +def test_bad_capture_identity_is_rejected_before_any_math_or_output(tmp_path, monkeypatch, fault): + _, rows, raw = write_actual_capture(tmp_path) + if fault == "second_session": + rows.append(deepcopy(rows[0])) + elif fault == "profile": + rows[0]["profile_id"] = "O12/right/foreign/v1" + elif fault == "image_content": + rows[2]["tags"]["thumb_ip"]["corners_xy"][0][0] += 1. + elif fault == "source_identity": + rows[2]["session_epoch"] += 1 + else: + rows.append(deepcopy(rows[2])) + rows[-1]["tags"]["thumb_ip"]["corners_xy"][0][0] += 1. + raw.write_text("".join(json.dumps(row)+"\n" for row in rows)) + before = raw.read_bytes() + monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images", + lambda *args, **kwargs: pytest.fail("invalid evidence reached image solver")) + with pytest.raises(ValueError, match="diagnostic_analysis_"): + analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw) + assert raw.read_bytes() == before + assert not (tmp_path / "diagnostic_analysis.json").exists() + + +def test_frozen_root_is_preserved_while_moving_candidates_remain_counterfactual(): + fixture = recorded_fixture("192637") + profile = load_bundled_hand_profile("o6_right_8") + rows = [fixture["session_start"], *fixture["records"], fixture["recorded_initialization"]] + _, stages = analysis._stages(profile, rows) + _, relations, records = stages[0] + records = deepcopy(records) + for row in records: + for role in ("thumb_pitch", "thumb_ip"): + row["tags"][role]["candidate_diagnostics"]["branch_frozen"] = True + before = deepcopy(records) + frames = analysis.image_frames(records, relations) + for frame, original in zip(frames, records, strict=True): + poses = {item.role: item for item in frame.evidence.roles} + root = poses["front_base"] + assert root.frozen is True and len(root.candidates) == 1 + assert root.candidates[0].quaternion_xyzw == tuple(original["tags"]["front_base"]["selected_pose"]["quaternion_xyzw"]) + for role in ("thumb_pitch", "thumb_ip"): + assert poses[role].frozen is False + assert len(poses[role].candidates) == len(original["tags"][role]["reprojection_valid_candidates"]) + assert records == before + + +def test_source_change_during_analysis_prevents_publication_of_diagnostic_output(tmp_path, monkeypatch): + _, _, raw = write_actual_capture(tmp_path) + calls = mock_image_analysis(monkeypatch, lambda: raw.write_text(raw.read_text()+"\n")) + with pytest.raises(ValueError, match="source_changed_during_read"): + analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw) + assert len(calls) == 1 + assert not (tmp_path / "diagnostic_analysis.json").exists() + + +@pytest.mark.parametrize("error_type", [ValueError, RuntimeError]) +def test_failed_analysis_preserves_completed_capture_and_never_claims_accuracy(tmp_path, monkeypatch, error_type): + _, _, raw = write_actual_capture(tmp_path) + before = raw.read_bytes() + capture = tmp_path / "diagnostic_capture.json" + capture.write_text('{"result": "DIAGNOSTIC_COMPLETE"}') + original = capture.read_bytes() + def fail(*args, **kwargs): + raise error_type("test_analysis_failure") + monkeypatch.setattr(analysis, "analyze_capture", fail) + report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path) + assert report["status"] == "failed" + assert report["accuracy_validated"] is report["publication_allowed"] is False + assert raw.read_bytes() == before and capture.read_bytes() == original + assert not list(tmp_path.rglob("*.urdf")) + + +def test_unrelated_camera_sweep_is_not_reported_as_failed_joint_geometry(tmp_path, monkeypatch): + _, rows, raw = write_actual_capture(tmp_path) + unrelated = deepcopy(rows[2]) + unrelated.update(kind="diagnostic_observation_frame", sample_phase="sweep", + task_name="thumb_yaw_top", cycle=0, direction="decreasing", attempt=1) + unrelated["image_stamp_ns"] += 1 + for item in unrelated["tags"].values(): + item["candidate_diagnostics"]["observation_stamp_ns"] = unrelated["image_stamp_ns"] + # The front camera was running for a later task in this same diagnostic; + # it has no measurement relation in the top-view yaw task. + rows.append(unrelated) + raw.write_text("".join(json.dumps(row)+"\n" for row in rows)) + calls = mock_image_analysis(monkeypatch) + report = analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw) + assert len(calls) == len(report["stages"]) == 1 + assert report["stages"][0]["task_name"] == "thumb_pitch_ip_front" + + +def test_fourth_round_sweep_never_enters_diagnostic_training(tmp_path, monkeypatch): + fixture = recorded_fixture("175815") + rows = [fixture["session_start"], *fixture["records"]] + for row in rows[1:]: + row["cycle"] = 3 + raw = tmp_path / "raw_samples.jsonl" + raw.write_text("".join(json.dumps(row)+"\n" for row in rows)) + before = raw.read_bytes() + monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images", + lambda *args, **kwargs: pytest.fail("holdout images reached optimizer")) + with pytest.raises(ValueError, match="refuses_holdout_training"): + analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw) + assert raw.read_bytes() == before + + +@pytest.mark.parametrize("collision", ["filename", "symlink", "hardlink"]) +def test_analysis_outputs_cannot_replace_raw_input(tmp_path, monkeypatch, collision): + import os + + _, _, raw = write_actual_capture(tmp_path) + before = raw.read_bytes() + target = tmp_path / "diagnostic_analysis.md" + if collision == "filename": + raw.rename(target) + raw = target + elif collision == "symlink": + target.symlink_to(raw) + else: + os.link(raw, target) + monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images", + lambda *args, **kwargs: pytest.fail("output collision reached optimizer")) + with pytest.raises(ValueError, match="output_overwrites_source"): + analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw) + assert raw.read_bytes() == before + + +def test_failure_report_cannot_bypass_output_source_collision_guard(tmp_path): + _, _, raw = write_actual_capture(tmp_path) + before = raw.read_bytes() + (tmp_path / "diagnostic_analysis.md").symlink_to(raw) + report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path) + assert report["status"] == "failed" + assert report["accuracy_validated"] is report["publication_allowed"] is False + assert raw.read_bytes() == before + + +def test_failure_report_write_error_does_not_escape_into_capture_failure(tmp_path, monkeypatch, capsys): + _, _, raw = write_actual_capture(tmp_path) + before = raw.read_bytes() + def fail(*args, **kwargs): + raise OSError("diagnostic_output_unwritable") + monkeypatch.setattr(analysis, "analyze_capture", fail) + monkeypatch.setattr(analysis, "atomic_write_json", fail) + report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path) + assert report["status"] == "failed" + assert report["publication_allowed"] is report["accuracy_validated"] is False + assert raw.read_bytes() == before + assert "原始采集状态保持不变" in capsys.readouterr().out diff --git a/src/linkerhand_calibration/test/test_diagnostic_coordinator.py b/src/linkerhand_calibration/test/test_diagnostic_coordinator.py new file mode 100644 index 0000000..e7b2deb --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_coordinator.py @@ -0,0 +1,383 @@ +"""Real coordinator diagnostics with projected Tags and an in-memory SDK clock.""" + +from concurrent.futures import ThreadPoolExecutor +import hashlib +import json +from dataclasses import replace +from pathlib import Path +from threading import Event + +import cv2 +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.pnp import square_object_points +from linkerhand_calibration.core.geometry.tag_pose.tracking import SquareTagPoseTracker +from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection +from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready + + +THUMB_JOINTS = {"rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_thumb_ip"} +TERMINAL_PHASES = {Phase.DIAGNOSTIC_COMPLETE, Phase.PAUSED, Phase.ABORTED, Phase.FAILED} + + +def projected_frame(host, view, feedback, *, stationary_centers=False): + """Project fixed mounts on two synthetic revolute chains. + + The previous transport fixture rotated each Tag about its own fixed centre. + That is deliberately retained below as an unobservable counterexample. The + complete-flow fixture now supplies the physical moving-centre evidence that + pre-freeze branch confirmation requires; no recorded baseline is rewritten. + """ + yaw = (255 - feedback[1]) / 255 * .8 + pitch = (255 - feedback[0]) / 255 * .9 + ip = (255 - feedback[0]) / 255 * .5 + base_rotation = Rotation.from_euler("xyz", [.2, .1, 0]) + base_center = np.asarray([-.07, -.045, .4]) + yaw_rotation = base_rotation * Rotation.from_rotvec([0, 0, yaw]) + pitch_rotation = base_rotation * Rotation.from_rotvec([0, 0, pitch]) + ip_rotation = pitch_rotation * Rotation.from_rotvec([-ip, 0, 0]) + pitch_joint = base_center + base_rotation.apply([0.015, 0.02, 0]) + ip_joint = pitch_joint + pitch_rotation.apply([0.06, 0, 0]) + mounts = { + "thumb_yaw": (yaw_rotation, base_center + yaw_rotation.apply([.06, 0, 0])), + "thumb_pitch": (pitch_rotation, pitch_joint + pitch_rotation.apply([.03, 0, 0])), + "thumb_ip": (ip_rotation, ip_joint + ip_rotation.apply([0, .04, 0])), + } + detections = [] + for index, tag in enumerate(next(v for v in host.profile.vision.views if v.name == view).tags): + if not tag.fixed_reference and tag.role not in mounts: + continue # Unselected pinky Tags stay hidden throughout the test. + rotation, center = mounts.get(tag.role, (base_rotation, base_center)) + if stationary_centers: + center = np.asarray([-.07 + .07 * index, -.045 + .025 * index, .4]) + rvec = rotation.as_rotvec() + corners, _ = cv2.projectPoints(square_object_points(tag.size_m), rvec, center, + host.cameras.matrices[view], np.zeros(5)) + detections.append(TagDetection(tag.tag_id, + tuple(tuple(float(value) for value in row) for row in corners.reshape(4, 2)), 0, 100.)) + return DetectionInput(view, host.ports.clock_ns(), tuple(detections)) + + +def start_diagnostic(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + ready(host, clock) + for view in set(host.profile.vision.view_names) - set(host.required_observation_views): + for mapping in (host.cameras.matrices, host.cameras.image_sizes, + host.cameras.info_received_at, host.cameras.detections_received_at): + mapping.pop(view, None) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + monkeypatch.setattr(host.finalization, "start", lambda *a, **kw: pytest.fail("diagnostic started finalizer")) + assert host.start().success + return host, clock + + +def advance(host, clock, *, images=True): + clock.now += .04 + feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + host.tick() + if images: + task = next((task for task in host.profile.motion.tasks + if host._motion is not None and task.key == host._motion.task_key), None) + views = (task.view,) if task is not None else host.required_observation_views + for view in views: + host.receive_detections(projected_frame(host, view, feedback)) + + +def run_until(host, clock, predicate, *, images=True): + for _ in range(8000): + if predicate(): + return + if host.execution.session.phase in TERMINAL_PHASES: + pytest.fail(f"premature terminal state: {host.state}: {host.reason}") + advance(host, clock, images=images) + pytest.fail(f"virtual capture timed out: {host.state}: {host.reason}") + + +def assert_terminal_is_idle(host, clock, monkeypatch): + before = (len(clock.positions), len(clock.settings), len(host.raw_records), host.raw_path.read_bytes()) + monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: pytest.fail("terminal observation consumed")) + for _ in range(5): + advance(host, clock) + assert (len(clock.positions), len(clock.settings), len(host.raw_records), host.raw_path.read_bytes()) == before + + +@pytest.mark.parametrize("diagnostic", [True, False]) +def test_legacy_3d_stationary_centers_keep_raw_without_confirming_zero(tmp_path, monkeypatch, diagnostic): + import sys + + original = projected_frame + monkeypatch.setattr(sys.modules[__name__], "projected_frame", + lambda host, view, feedback: original(host, view, feedback, stationary_centers=True)) + if diagnostic: + host, clock = start_diagnostic(tmp_path, monkeypatch) + else: + host, clock = coordinator_fixture(tmp_path) + ready(host, clock) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + assert host.start().success + # Historical 3D candidate geometry requires center translation. The new + # image solver uses all corners and may observe rotation around a Tag's + # center; this test retains the old diagnostic/compatibility contract. + host.branch_initialization.image_geometry = False + try: + run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES) + if diagnostic: + assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE + assert set(host.execution.diagnostic_zero_attempts) == THUMB_JOINTS + attempts = [row for row in host.raw_records if row.get("kind") == "diagnostic_joint_zero_attempt"] + assert len(attempts) == 2 # One visit per group across both directions. + assert all(row["no_zero_locked"] and not row["is_accepted_joint_sample"] for row in attempts) + assert len(host.execution.completed_units) == 4 + else: + assert host.execution.session.phase == Phase.PAUSED + assert host.reason == "joint_zero_motion_unresolved:view=top:joints=rh_thumb_cmc_yaw:motion_geometry_unresolved" + assert not host.execution.diagnostic_zero_attempts + assert not host.execution.zero_references + reports = [r for r in host.raw_records if r.get("kind") == "motion_branch_initialization"] + assert reports and all(r["status"] == "unresolved" for r in reports) + assert any(q["reason"] == "insufficient_pivot_translation" + for r in reports for h in r["hypotheses"] for q in h["relations"]) + assert not any(row.get("kind") == "joint_sample" for row in host.raw_records) + assert not host.finalization.started and not host.final_json and not host.final_urdf + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + host.close() + + +@pytest.mark.parametrize("unused_side", ["absent", "ambiguous_recorded_tag"]) +def test_complete_thumb_diagnostic_uses_real_capture_and_never_publishes(tmp_path, monkeypatch, unused_side): + host, clock = start_diagnostic(tmp_path, monkeypatch) + # This historical transport fixture assumes its 3D PnP hinge solution is + # available; its nearly frontal mirror families need not be identifiable + # by the new image optimizer. Preserve the original capture/fault contract + # here. test_image_motion_live_flow covers the complete production image + # path with recorded preparation and directional observations. + host.branch_initialization.image_geometry = False + if unused_side == "ambiguous_recorded_tag": + fixture = json.loads((Path(__file__).with_name("fixtures") / + "o6_side_base_20260914_190619.json").read_text()) + item = fixture["observation"]["pnp_observation_evidence"] + matrix = np.asarray(fixture["camera_model"]["camera_matrix"]) + corners = item["corners_xy"] + # First reproduce the real stationary ambiguity with the unchanged + # tracker. Side transport then delivers the same corners during this + # run, but must never become a reference dependency of the thumb. + tracker = SquareTagPoseTracker() + for i in range(12): + pose, reason = tracker.estimate("side_base", corners, + tag_size_m=item["tag_size_m"], camera_matrix=matrix, stamp_ns=(i+1)*40_000_000) + assert pose is None and reason == "pose_branch_unconfirmed" + assert tracker.observation_snapshot("side_base", 12*40_000_000)[1]["branch_revision"] == 0 + receive = host.receive_detections + def with_unused_side(frame): + receive(DetectionInput("side", frame.stamp_ns, + (TagDetection(3, tuple(tuple(p) for p in corners), 0, 100.),))) + receive(frame) + monkeypatch.setattr(host, "receive_detections", with_unused_side) + try: + run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE) + assert set(host.execution.zero_references) == THUMB_JOINTS + zero_rows = [row for row in host.raw_records if row.get("kind") == "joint_zero_reference"] + assert len(zero_rows) == 3 + sweeps = [row for row in host.raw_records if row.get("kind") == "joint_sample"] + assert sweeps + assert {row["joint"] for row in sweeps} == THUMB_JOINTS + assert {row["sample_phase"] for row in sweeps} == {"sweep"} + assert {row["cycle"] for row in sweeps} == {0} + assert {row["direction"] for row in sweeps} == {"increasing", "decreasing"} + assert all(row["pnp_observation_evidence"] for row in sweeps) + assert len(host.execution.completed_units) == 4 + assert all(tuple(command[2:]) == host.profile.command.baseline_values[2:] for command in clock.positions) + assert host.last_command == host.profile.command.baseline_values + assert host.latest_feedback == host.profile.command.baseline_values + assert not host.finalization.started + assert not host.final_json and not host.final_urdf + assert not list(tmp_path.rglob("*.urdf")) + assert not any(path.is_symlink() for path in tmp_path.rglob("*")) + report_path = host.parameters.session_dir / "diagnostic_capture.json" + report_text = report_path.read_text() + report = json.loads(report_text) + assert report["result"] == "DIAGNOSTIC_COMPLETE" + assert report["publication_allowed"] is False + assert report["accuracy_validated"] is False + assert report["raw_samples_sha256"] == hashlib.sha256(host.raw_path.read_bytes()).hexdigest() + assert set(report["joint_zero_references"]) == THUMB_JOINTS + assert len(report["completed_units"]) == 4 + assert len(report["unit_quality"]) == 4 + assert all(row["passed"] and row["accepted_pose_quality"]["passed"] + for row in report["unit_quality"]) + assert report["accepted_pose_quality_incomplete_attempts"] == 0 + assert report["pose_step_summary"] + assert {key: report["protected_inputs"][key] for key in host.parameters.protected_inputs} == host.parameters.protected_inputs + captured = [json.loads(line) for line in host.raw_path.read_text().splitlines()] + # A one-round thumb diagnostic proves its recorded references, not + # full-hand coverage or the formal four-round accuracy contract. + evidence = validate_motion_provenance(host.profile, captured, required=True) + assert evidence["motion_evidence_verified"] is True + assert evidence["is_accuracy_certificate"] is False + locked = next(row for row in captured if row["kind"] == "fixed_base_reference_locked") + assert len([row for row in captured if row["kind"] == "fixed_base_reference_locked"]) == 1 + assert set(locked["fixed_corners_by_view"]) == {"front", "top"} + assert set(locked["fixed_poses"]) == {"front", "top"} + assert set(host.capture.locked_reference_views()) == {"front", "top"} + assert not any(row.get("view") == "side" for row in captured) + assert set(host.trackers) == {"front", "top"} + start = next(row for row in captured if row["kind"] == "session_start") + assert start["required_observation_views"] == ["front", "top"] + assert report["protected_inputs"] == locked["protected_hashes"] + assert len(report["protected_inputs"]["intrinsics_sha256"]) == 64 + snapshot = host.snapshot().calibration + assert snapshot.phase == "DIAGNOSTIC_COMPLETE" + assert snapshot.overall_progress_01 == 1. + assert snapshot.task.cycle_count == 1 + assert_terminal_is_idle(host, clock, monkeypatch) + assert not host.abort().success + assert not host.start().success + assert report_path.read_text() == report_text + finally: + host.close() + + +@pytest.mark.parametrize("fault", ["ambiguous_pose", "missing_tag"]) +def test_diagnostic_retains_rejected_candidates_but_missing_tag_still_retries_once(tmp_path, monkeypatch, fault): + host, clock = start_diagnostic(tmp_path, monkeypatch) + # This historical transport fixture assumes its 3D PnP hinge solution is + # available; its nearly frontal mirror families need not be identifiable + # by the new image optimizer. Preserve the original capture/fault contract + # here. test_image_motion_live_flow covers the complete production image + # path with recorded preparation and directional observations. + host.branch_initialization.image_geometry = False + commit = SquareTagPoseTracker.commit_motion_frame + + def affected(role): + return role == "thumb_ip" and host._motion is not None and host._motion.phase == "sweep" + + # Inject rejection at the new atomic delivery boundary after real PnP and + # geometric selection. The geometry tests separately exercise ambiguity; + # this transport test still requires retaining the original raw candidates. + def reject_motion(selections, **kwargs): + if any(affected(item.snapshot.role) for item in selections): + return SquareTagPoseTracker.reject_motion_frame( + tuple(item.snapshot for item in selections), "pose_branch_ambiguous") + return commit(selections, **kwargs) + + receive = host.receive_detections + def hide_tag(frame): + if host._motion is not None and host._motion.phase == "sweep" and frame.view == "front": + frame = replace(frame, detections=tuple(tag for tag in frame.detections if tag.tag_id != 2)) + return receive(frame) + + if fault == "ambiguous_pose": + monkeypatch.setattr(SquareTagPoseTracker, "commit_motion_frame", staticmethod(reject_motion)) + else: + monkeypatch.setattr(host, "receive_detections", hide_tag) + try: + run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES) + assert not host.finalization.started + assert not host.final_json and not host.final_urdf + assert len([row for row in host.raw_records if row.get("kind") == "joint_zero_reference"]) == 3 + assert not any(row.get("kind") == "joint_sample" and row.get("joint") == "rh_thumb_ip" + for row in host.raw_records) + checks = [row for row in host.raw_records if row.get("kind") == "scan_unit_complete" + and row["task_name"] == "thumb_pitch_ip_front"] + assert len(checks) == 2 + assert all(row["accepted_pose_quality"]["passed"] is False for row in checks) + if fault == "missing_tag": + assert host.execution.session.phase == Phase.PAUSED + assert "sweep_quality_failed" in host.reason + assert host.snapshot().calibration.pause.details["observation_timing"] == checks[-1]["observation_timing"] + assert [row["attempt"] for row in checks] == [1, 2] + assert all(row["passed"] is False for row in checks) + assert not (host.parameters.session_dir / "diagnostic_capture.json").exists() + else: + assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE + assert [row["attempt"] for row in checks] == [1, 1] + assert all(row["passed"] for row in checks) + frames = [row for row in host.raw_records if row.get("kind") == "diagnostic_observation_frame" + and row["view"] == "front" and row["sample_phase"] == "sweep"] + assert frames and all(row["tags"]["thumb_ip"]["selected_pose"] is None for row in frames) + assert all(row["tags"]["thumb_ip"]["reprojection_valid_candidates"] for row in frames) + assert {row["direction"] for row in frames} == {"increasing", "decreasing"} + report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text()) + assert report["accepted_pose_quality_incomplete_attempts"] == 2 + assert not report["accuracy_validated"] and not report["publication_allowed"] + finally: + host.close() + + +@pytest.mark.parametrize("action", ["pause", "abort"]) +def test_interrupted_thumb_diagnostic_holds_without_success_report(tmp_path, monkeypatch, action): + host, clock = start_diagnostic(tmp_path, monkeypatch) + try: + run_until(host, clock, lambda: host._motion is not None and host._motion.phase == "sweep" + and host.last_command[1] < 240) + if action == "pause": + host._pause("test_visual_fault") + assert host.execution.session.phase == Phase.PAUSED + else: + assert host.abort().success + assert host.execution.session.phase == Phase.ABORTED + assert host.last_command != host.profile.command.baseline_values + assert not (host.parameters.session_dir / "diagnostic_capture.json").exists() + assert not host.finalization.started + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + host.close() + + +def test_diagnostic_report_write_failure_is_terminal_and_cannot_restart_motion(tmp_path, monkeypatch): + from linkerhand_calibration.runtime import coordinator as coordinator_module + + original_write = coordinator_module.atomic_write_json + def write(path, payload): + if path.name == "diagnostic_capture.json": + raise OSError("synthetic report write failure") + return original_write(path, payload) + monkeypatch.setattr(coordinator_module, "atomic_write_json", write) + host, clock = start_diagnostic(tmp_path, monkeypatch) + try: + run_until(host, clock, lambda: host.execution.session.phase == Phase.FAILED) + assert "diagnostic_capture_report_failed:synthetic report write failure" == host.reason + assert not (host.parameters.session_dir / "diagnostic_capture.json").exists() + assert not host.finalization.started + assert not host.final_json and not host.final_urdf + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + host.close() + + +def test_pose_started_before_diagnostic_completion_cannot_change_hashed_capture(tmp_path, monkeypatch): + host, clock = start_diagnostic(tmp_path, monkeypatch) + entered, release = Event(), Event() + try: + run_until(host, clock, lambda: host.execution.session.phase == Phase.RETURN_BASELINE) + def consume(*args, **kwargs): + entered.set() + assert release.wait(5) + return ({"kind": "late_sample"},), None + monkeypatch.setattr(host.capture, "consume", consume) + with ThreadPoolExecutor(1) as pool: + future = pool.submit(host.receive_detections, + DetectionInput("front", host.ports.clock_ns(), ())) + assert entered.wait(2) + epoch = host._observation_epoch + run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE, + images=False) + assert host._observation_epoch > epoch + report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text()) + before = host.raw_path.read_bytes() + release.set() + future.result(timeout=2) + assert all(row.get("kind") != "late_sample" for row in host.raw_records) + assert host.raw_path.read_bytes() == before + assert hashlib.sha256(before).hexdigest() == report["raw_samples_sha256"] + finally: + release.set() + host.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py b/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py new file mode 100644 index 0000000..7d0f7b0 --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py @@ -0,0 +1,215 @@ +"""Only the selected diagnostic's camera dependencies may gate its session.""" + +from dataclasses import replace +import hashlib +import json + +import numpy as np +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.diagnostic_capture import ( + DiagnosticCapturePlan, + resolve_diagnostic_capture, +) +from linkerhand_calibration.runtime.inputs import CameraModelInput, DetectionInput +from linkerhand_calibration.runtime.observation_scope import required_observation_views +from linkerhand_calibration.runtime.session import CalibrationPhase, CalibrationSession +from runtime_host_fixture import coordinator_fixture, ready + + +@pytest.mark.parametrize("profile_name", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def test_formal_sessions_keep_every_declared_camera(profile_name): + profile = load_bundled_hand_profile(profile_name) + assert required_observation_views(profile) == profile.vision.view_names + assert CalibrationSession(profile).required_observation_views == profile.vision.view_names + + +def test_o6_thumb_dependencies_are_front_and_top(): + profile = load_bundled_hand_profile("o6_right_8") + plan = resolve_diagnostic_capture(profile, "o6_thumb") + assert required_observation_views(profile, plan) == ("front", "top") + assert CalibrationSession(profile, diagnostic_capture=plan).required_observation_views == ("front", "top") + + +def test_virtual_task_and_camera_names_follow_measurements_including_secondary_view(): + profile = load_bundled_hand_profile("o6_right_8") + renamed = {"front": "inspect", "side": "oblique", "top": "overhead"} + views = {view.name: replace(view, name=renamed[view.name]) for view in profile.vision.views} + tasks = tuple(replace(task, key=f"virtual_{task.key}", view=renamed[task.view]) + for task in reversed(profile.motion.tasks)) + measurements = {name: replace(spec, view=renamed.get(spec.view, spec.view)) + for name, spec in profile.measurement.measurements.items()} + yaw = "rh_thumb_cmc_yaw" + secondary = "yaw_from_oblique_camera" + measurements[secondary] = replace(measurements[yaw], joint=secondary, view="oblique") + virtual = replace(profile, + vision=replace(profile.vision, views=(views["side"], views["top"], views["front"]), + extrinsic_reference_view="inspect"), + motion=replace(profile.motion, tasks=tasks), + measurement=replace(profile.measurement, measurements=measurements, + cross_view_sources={yaw: secondary})) + task = next(task for task in tasks if yaw in task.joints) + plan = DiagnosticCapturePlan("virtual_selection", (task.key,)) + # Order comes from the rig, while inclusion comes from both observations. + assert required_observation_views(virtual, plan) == ("oblique", "overhead") + assert required_observation_views(virtual) == ("oblique", "overhead", "inspect") + + +def _remove_camera(host, view): + for mapping in (host.cameras.matrices, host.cameras.image_sizes, + host.cameras.info_received_at, host.cameras.detections_received_at, + host.cameras.errors, host.cameras.models): + mapping.pop(view, None) + + +def _invalid_camera_info(): + # A well-shaped model with a different identity from the protected cameras. + return CameraModelInput(640, 480, + (500., 0., 320., 0., 500., 240., 0., 0., 1.), (), + tuple(np.eye(3).flat), + (500., 0., 320., 0., 0., 500., 240., 0., 0., 0., 1., 0.)) + + +def test_missing_side_pipeline_does_not_block_thumb_ready_or_start(tmp_path): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + ready(host, clock) + _remove_camera(host, "side") + host.tick() + assert host.execution.session.phase == CalibrationPhase.READY + devices = host._device_status(clock.now) + assert devices.ready + assert set(devices.cameras) == {"front", "top"} + assert host.start().success + assert host.reference_lock.fixed_tag_by_view == {"front": 0, "top": 6} + preflight = json.loads((host.parameters.session_dir / "camera_preflight.json").read_text()) + assert set(preflight["configuration"]) == {"front", "top"} + finally: + host.close() + + +@pytest.mark.parametrize("view", ["front", "top"]) +@pytest.mark.parametrize("fault", ["info_missing", "detections_stale", "invalid_configuration"]) +def test_required_camera_fault_after_ready_blocks_start_without_motion(tmp_path, view, fault): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + ready(host, clock) + if fault == "info_missing": + host.cameras.info_received_at.pop(view) + elif fault == "detections_stale": + host.cameras.detections_received_at[view] = clock.now - 2.01 + else: + host.receive_camera_info(view, _invalid_camera_info()) + assert not host.start().success + assert host.execution.session.phase == CalibrationPhase.WAIT_DEVICE + assert view in host.reason + assert not host.started + assert not clock.positions + assert not clock.settings + finally: + host.close() + + +@pytest.mark.parametrize("view", ["front", "top"]) +def test_required_camera_model_changes_still_pause_started_diagnostic(tmp_path, view): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + ready(host, clock) + assert host.start().success + host.receive_camera_info(view, _invalid_camera_info()) + assert host.execution.session.phase == CalibrationPhase.PAUSED + assert host.reason.startswith(f"protected_camera_model_changed:{view}:") + assert not host.final_json and not host.final_urdf + finally: + host.close() + + +def test_side_callbacks_cannot_change_diagnostic_evidence_or_pause(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + ready(host, clock) + assert host.start().success + host.reference_lock.start_locking() + corners = np.asarray([[10., 10.], [30., 10.], [30., 30.], [10., 30.]]) + for view, tag in host.reference_lock.fixed_tag_by_view.items(): + for _ in range(host.reference_lock.minimum_frames): + host.reference_lock.observe(view, tag, corners) + assert host.reference_lock.locked + fingerprint = host.reference_lock.fingerprint() + before = host.raw_path.read_bytes() + seen = dict(host.cameras.detections_received_at) + matrices = {view: matrix.copy() for view, matrix in host.cameras.matrices.items()} + errors = dict(host.cameras.errors) + phase = host.execution.session.phase + monkeypatch.setattr(host.capture, "consume", + lambda *args, **kwargs: pytest.fail("unselected side observation entered capture")) + clock.now += .1 + host.receive_detections(DetectionInput("side", host.ports.clock_ns(), ())) + host.receive_camera_info("side", _invalid_camera_info()) + assert host.execution.session.phase == phase + assert host.cameras.detections_received_at == seen + assert host.cameras.errors == errors + for view, matrix in matrices.items(): + np.testing.assert_array_equal(host.cameras.matrices[view], matrix) + assert host.reference_lock.fingerprint() == fingerprint + assert host.raw_path.read_bytes() == before + assert [row["kind"] for row in host.raw_records] == ["session_start"] + finally: + host.close() + + +def test_diagnostic_intrinsics_fingerprint_ignores_unselected_cached_camera(tmp_path): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + ready(host, clock) + assert host.start().success + host.reference_lock.start_locking() + corners = np.asarray([[10., 10.], [30., 10.], [30., 30.], [10., 30.]]) + for view, tag in host.reference_lock.fixed_tag_by_view.items(): + for _ in range(host.reference_lock.minimum_frames): + host.reference_lock.observe(view, tag, corners) + host._save_reference() + first = host._current_fingerprint + expected = hashlib.sha256(json.dumps({view: host.cameras.matrices[view].tolist() + for view in ("front", "top")}, sort_keys=True).encode()).hexdigest() + assert first["protected_hashes"]["intrinsics_sha256"] == expected + assert set(first["fixed_corners_by_view"]) == {"front", "top"} + # Cached data can predate Start; its presence cannot alter this session's identity. + host.cameras.matrices["side"] = 9. * np.eye(3) + host._save_reference() + assert host._current_fingerprint == first + finally: + host.close() + + +def test_formal_side_pipeline_remains_a_required_device(tmp_path): + host, clock = coordinator_fixture(tmp_path) + try: + ready(host, clock) + _remove_camera(host, "side") + host.tick() + assert host.execution.session.phase == CalibrationPhase.WAIT_DEVICE + assert "side" in host.reason + assert not host.start().success + assert set(host.reference_lock.fixed_tag_by_view) == {"front", "side", "top"} + finally: + host.close() + + +def test_formal_side_callbacks_still_enter_capture_and_check_camera_identity(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + try: + ready(host, clock) + assert host.start().success + calls = [] + monkeypatch.setattr(host.capture, "consume", lambda frame, *args, **kwargs: calls.append(frame) or ([], None)) + clock.now += .1 + host.receive_detections(DetectionInput("side", host.ports.clock_ns(), ())) + assert [frame.view for frame in calls] == ["side"] + assert host.cameras.detections_received_at["side"] == clock.now + host.receive_camera_info("side", _invalid_camera_info()) + assert host.execution.session.phase == CalibrationPhase.PAUSED + assert host.reason.startswith("protected_camera_model_changed:side:") + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_provenance.py b/src/linkerhand_calibration/test/test_diagnostic_provenance.py new file mode 100644 index 0000000..68fc51e --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_provenance.py @@ -0,0 +1,98 @@ +"""A diagnostic thumb capture cannot become a production fit or checkpoint.""" + +from copy import deepcopy +import json + +import pytest + +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.runtime.artifacts.replay import validate_capture_provenance +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION +from linkerhand_calibration.runtime.resume import discover_resume_candidate + + +DIAGNOSTIC = { + "mode": "o6_thumb", + "task_keys": ["thumb_yaw_top", "thumb_pitch_ip_front"], + "cycles": [0], +} + + +@pytest.mark.parametrize("marker", [DIAGNOSTIC, True, "o6_thumb"]) +def test_offline_replay_rejects_diagnostic_header_before_other_provenance(marker): + rows = [{"kind": "session_start", "diagnostic_capture": marker}] + before = deepcopy(rows) + with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"): + validate_capture_provenance(None, "TEST", {}, rows) + assert rows == before + + +@pytest.mark.parametrize("record", [ + {"kind": "diagnostic_observation_frame"}, + {"kind": "scan_unit_complete", "quality_scope": "diagnostic_observation_coverage"}, +]) +def test_removing_header_does_not_promote_diagnostic_records(record): + with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"): + validate_capture_provenance(None, "TEST", {}, [record]) + + +@pytest.mark.parametrize("publish", [False, True]) +def test_direct_finalizer_rejects_diagnostic_before_files_or_fitting(tmp_path, publish): + source = tmp_path / "original.urdf" + source.write_text('', encoding="utf-8") + original = source.read_bytes() + previous = tmp_path / "previous_release" + previous.mkdir() + pointer = tmp_path / "latest_passed" + pointer.symlink_to(previous, target_is_directory=True) + directory = tmp_path / "diagnostic_output" + rows = [{"kind": "session_start", "diagnostic_capture": DIAGNOSTIC}] + with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"): + finalize_profile_session(profile=None, session_dir=directory, + serial_number="TEST", source_urdf=source, protected_inputs={}, + records=rows, publish=publish, + standard_loader=lambda _: pytest.fail("diagnostic data must never reach a loader")) + assert not directory.exists() + assert source.read_bytes() == original + assert pointer.resolve() == previous + + +def _checkpoint(directory, *, diagnostic=False): + directory.mkdir() + header = { + "kind": "session_start", "profile_id": "O6/right/layout/v1", + "serial_number": "TEST", "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": CAPTURE_SCHEDULE_VERSION, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, + "protected_hashes": {"source": "a" * 64}, + } + if diagnostic: + header["diagnostic_capture"] = DIAGNOSTIC + records = [header, {"kind": "fixed_base_reference_locked"}, + {"kind": "scan_unit_complete", "passed": True}] + path = directory / "raw_samples.jsonl" + path.write_text("\n".join(json.dumps(row) for row in records), encoding="utf-8") + return path + + +@pytest.mark.parametrize("production_available", [False, True]) +def test_diagnostic_checkpoint_never_shadows_or_supplies_production_resume(tmp_path, production_available): + production = tmp_path / "01" + if production_available: + _checkpoint(production) + diagnostic = _checkpoint(tmp_path / "02", diagnostic=True) + before = diagnostic.read_bytes() + # Discovery must reject the raw header even if diagnostic_capture.json is + # absent and the recorded direction claims to have passed all quality gates. + found = discover_resume_candidate(tmp_path, profile_id="O6/right/layout/v1", + serial_number="TEST", protected_hashes={"source": "a" * 64}) + assert found == (production if production_available else None) + assert diagnostic.read_bytes() == before + + +def test_resume_discovery_checks_diagnostic_evidence_beyond_header(tmp_path): + path = _checkpoint(tmp_path / "01") + path.write_text(path.read_text() + '\n' + json.dumps({"kind": "diagnostic_observation_frame"})) + assert discover_resume_candidate(tmp_path, profile_id="O6/right/layout/v1", + serial_number="TEST", protected_hashes={"source": "a" * 64}) is None diff --git a/src/linkerhand_calibration/test/test_diagnostic_quality.py b/src/linkerhand_calibration/test/test_diagnostic_quality.py new file mode 100644 index 0000000..b112ffa --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_quality.py @@ -0,0 +1,111 @@ +"""Raw image coverage can finish a diagnosis but cannot certify joint poses.""" + +from copy import deepcopy + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.diagnostic_quality import evaluate_diagnostic_unit +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit + + +def raw_frames(profile, unit): + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + rows = [] + for index in range(129): + feedback = list(profile.command.baseline_values) + feedback[task.command_index] = unit.start + (unit.end - unit.start) * index / 128 + for view in profile.vision.views: + stamp = 1_000_000_000 + index * 40_000_000 + rows.append({"kind": "diagnostic_observation_frame", "view": view.name, + "task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction, + "attempt": 1, "sample_phase": "sweep", "image_stamp_ns": stamp, + "feedback_vector": feedback, "command_vector": list(feedback), + "command_unit": profile.command.unit, "is_accepted_joint_sample": False, + "tags": {tag.role: {"tag_role": tag.role, "tag_id": tag.tag_id, + "corners_xy": [[10, 10], [50, 10], [50, 50], [10, 50]], + "selected_pose": None, + "candidate_diagnostics": {"observation_stamp_ns": stamp, + "branch_status": "frozen_ambiguous"}} + for tag in view.tags if not tag.fixed_reference}}) + return rows + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def test_raw_corners_cover_motion_without_becoming_measured_joint_angles(layout): + profile = load_bundled_hand_profile(layout) + unit = CalibrationEngine(profile).scan_units()[0] + rows = raw_frames(profile, unit) + before = deepcopy(rows) + result = evaluate_diagnostic_unit(profile, unit, 1, rows) + assert result.observations.passed, result.observations.failures + assert not result.accepted_poses.passed + assert result.accepted_pose_report()["accuracy_validated"] is False + assert not evaluate_capture_unit(profile, unit, 1, rows, + first_cycle_spans={}, include_steady=False).passed + assert rows == before + assert all("joint" not in row and "observation_joint" not in row for row in rows) + + +@pytest.mark.parametrize("fault", ["missing", "parent_missing", "stale_image", "tag_id", + "tag_role", "corners", "feedback", "command", "unit", "view", "task", "cycle", + "direction", "attempt", "outside_range"]) +def test_raw_coverage_still_requires_current_correctly_bound_moving_tags_and_feedback(fault): + profile = load_bundled_hand_profile("o6_right_8") + unit = next(unit for unit in CalibrationEngine(profile).scan_units() + if unit.task_key == "thumb_pitch_ip_front") + rows = raw_frames(profile, unit) + for row in rows: + if row["view"] != "front": + continue + item = row["tags"]["thumb_ip"] + if fault == "missing": + del row["tags"]["thumb_ip"] + elif fault == "parent_missing": + del row["tags"]["thumb_pitch"] + elif fault == "stale_image": + item["candidate_diagnostics"]["observation_stamp_ns"] -= 1 + elif fault in {"tag_id", "tag_role", "corners"}: + item[{"corners": "corners_xy"}.get(fault, fault)] = { + "tag_id": 1, "tag_role": "thumb_pitch", "corners": None}[fault] + elif fault == "outside_range": + row["feedback_vector"][0] = -1 + else: + key, value = {"feedback": ("feedback_vector", None), "command": ("command_vector", []), + "unit": ("command_unit", "rad"), "view": ("view", "side"), + "task": ("task_name", "thumb_yaw_top"), "cycle": ("cycle", 1), + "direction": ("direction", "increasing"), "attempt": ("attempt", 2)}[fault] + row[key] = value + result = evaluate_diagnostic_unit(profile, unit, 1, rows) + assert not result.observations.passed + assert not result.accepted_poses.passed + + +def test_visible_tags_and_full_endpoint_span_cannot_hide_a_synchronization_gap(): + profile = load_bundled_hand_profile("o6_right_8") + unit = CalibrationEngine(profile).scan_units()[0] + rows = raw_frames(profile, unit) + for row in rows: + if row["view"] == "top" and 80 < row["feedback_vector"][1] < 130: + row["feedback_vector"] = None + result = evaluate_diagnostic_unit(profile, unit, 1, rows) + assert not result.observations.passed + assert any("maximum_gap=" in reason for reason in result.observations.failures) + metrics = next(iter(result.observations.metrics.values())) + assert metrics["feedback_span"] == 1. + assert metrics["allowed_maximum_bin_gap"] == 16 + timing = next(iter(result.observation_timing.values())) + assert timing["raw_frames"] == timing["visible_frames"] == 129 + assert timing["missing_feedback_frames"] > 0 + assert timing["maximum_synchronized_gap_seconds"] > timing["maximum_image_gap_seconds"] + + +@pytest.mark.parametrize("phase", ["steady", "joint_zero"]) +def test_non_sweep_diagnostic_images_cannot_fill_missing_sweep_coverage(phase): + profile = load_bundled_hand_profile("o6_right_8") + unit = CalibrationEngine(profile).scan_units()[0] + rows = [dict(row, sample_phase=phase) for row in raw_frames(profile, unit)] + result = evaluate_diagnostic_unit(profile, unit, 1, rows) + assert not result.observations.passed and not result.accepted_poses.passed + assert all(value["raw_frames"] == 0 for value in result.observation_timing.values()) diff --git a/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py b/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py new file mode 100644 index 0000000..7992245 --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py @@ -0,0 +1,218 @@ +"""Raw diagnostics survive unobservable geometry without manufacturing zeros.""" + +from copy import deepcopy +from concurrent.futures import ThreadPoolExecutor +from dataclasses import replace +import json +from threading import Event + +import pytest + +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionBranchResolution +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime import coordinator as coordinator_module +from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture +from linkerhand_calibration.runtime.diagnostic_reference import build_diagnostic_zero_attempt +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from test_diagnostic_coordinator import (start_diagnostic, run_until, TERMINAL_PHASES, + assert_terminal_is_idle, projected_frame) + + +FRONT_JOINTS = {"rh_thumb_cmc_pitch", "rh_thumb_ip"} + + +def unresolved_front(monkeypatch): + solve = coordinator_module.solve_initialization + monkeypatch.setattr(coordinator_module, "solve_initialization", lambda request: + MotionBranchResolution(False, "motion_geometry_unresolved") + if request.view == "front" else solve(request)) + + +@pytest.mark.parametrize("fault", ["geometry_only", "missing_sweep_tag", "missing_zero_tag", "unstable_feedback"]) +def test_unresolved_reference_diagnostic_obeys_raw_coverage_and_sdk_stability(tmp_path, monkeypatch, fault): + unresolved_front(monkeypatch) + host, clock = start_diagnostic(tmp_path, monkeypatch) + receive = host.receive_detections + def hide(frame): + phase = None if host._motion is None else host._motion.phase + if frame.view == "front" and ((fault == "missing_sweep_tag" and phase == "sweep") + or (fault == "missing_zero_tag" and phase == "joint_zero")): + frame = replace(frame, detections=tuple(tag for tag in frame.detections if tag.tag_id != 2)) + return receive(frame) + monkeypatch.setattr(host, "receive_detections", hide) + if fault == "unstable_feedback": + steady_ready = MotionExecution.steady_ready + monkeypatch.setattr(MotionExecution, "steady_ready", lambda segment, now: + False if segment.command.phase == "joint_zero" and "rh_thumb_ip" in segment.command.zero_joints + else steady_ready(segment, now)) + try: + run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES) + assert set(host.execution.zero_references) == {"rh_thumb_cmc_yaw"} + assert not any(row.get("kind") in {"joint_sample", "joint_zero_sample", "secondary_joint_sample", "pnp_candidate_frame"} + and row.get("view") == "front" for row in host.raw_records) + assert not host.finalization.started and not host.final_json and not host.final_urdf + attempts = [r for r in host.raw_records if r.get("kind") == "diagnostic_joint_zero_attempt"] + if fault in {"missing_zero_tag", "unstable_feedback"}: + assert host.execution.session.phase == Phase.PAUSED + assert not attempts and not host.execution.diagnostic_zero_attempts + expected = ("diagnostic_joint_zero_observations_missing" if fault == "missing_zero_tag" + else "diagnostic_joint_zero_feedback_unstable") + assert expected in host.reason + else: + assert len(attempts) == 1 + assert set(host.execution.diagnostic_zero_attempts) == FRONT_JOINTS + checks = [r for r in host.raw_records if r.get("kind") == "scan_unit_complete" and r["task_name"] == "thumb_pitch_ip_front"] + if fault == "missing_sweep_tag": + assert host.execution.session.phase == Phase.PAUSED + assert [r["attempt"] for r in checks] == [1, 2] + assert all(not r["passed"] for r in checks) + else: + assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE + assert len(host.execution.completed_units) == 4 + assert len(checks) == 2 and all(r["passed"] for r in checks) + assert all(not r["accepted_pose_quality"]["passed"] for r in checks) + raw = [r for r in host.raw_records if r.get("kind") == "diagnostic_observation_frame" + and r.get("view") == "front" and r.get("sample_phase") == "sweep"] + # Independent tracking can recover, but without a zero its + # selected pose is still only a diagnostic observation. + assert any(item["selected_pose"] is not None for row in raw for role, item in row["tags"].items() + if role in {"thumb_pitch", "thumb_ip"}) + report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text()) + assert not report["accuracy_validated"] and not report["publication_allowed"] + with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"): + reject_diagnostic_capture(host.raw_records) + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + host.close() + + +def raw_reference(): + profile = load_bundled_hand_profile("o6_right_8") + name = "rh_thumb_cmc_yaw" + spec = profile.motion.joint_zero_references[name] + motion = MotionCommand("joint_zero", spec.command, 1., task_key=spec.task_key, zero_joints=(name,)) + rows = [] + for stamp in range(1, profile.acquisition.fixed_reference_minimum_frames+1): + rows.append({"kind": "diagnostic_observation_frame", "sample_phase": "joint_zero", + "task_name": spec.task_key, "view": "top", "session_epoch": 2, "motion_version": 3, + "zero_joints": [name], "image_stamp_ns": stamp, "command_unit": profile.command.unit, + "command_vector": list(spec.command), "feedback_vector": list(spec.command), + "tags": {"thumb_yaw": {"tag_role": "thumb_yaw", "tag_id": 7, + "corners_xy": [[0., 0.], [20., 0.], [20., 20.], [0., 20.]], + "candidate_diagnostics": {"observation_stamp_ns": stamp}}}}) + return profile, motion, rows + + +@pytest.mark.parametrize("invalid", ["missing", "duplicate", "session", "motion", "stamp", "binding", "baseline", "feedback"]) +def test_raw_reference_attempt_requires_current_distinct_complete_images(invalid): + profile, motion, rows = raw_reference() + rows = deepcopy(rows) + if invalid == "missing": + rows.pop() + elif invalid == "duplicate": + rows[-1] = deepcopy(rows[-2]) + elif invalid in {"session", "motion"}: + rows[-1]["session_epoch" if invalid == "session" else "motion_version"] += 1 + elif invalid == "stamp": + rows[-1]["tags"]["thumb_yaw"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1 + elif invalid == "binding": + rows[-1]["tags"]["thumb_yaw"]["tag_id"] = 99 + elif invalid == "baseline": + rows[-1]["command_vector"][1] -= 1 + else: + rows[-1]["feedback_vector"] = None + with pytest.raises(ValueError, match="diagnostic_joint_zero_observations_missing"): + build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved", + session_epoch=2, motion_version=3) + + +def test_diagnostic_attempt_is_not_a_formal_reference_or_reusable_sample(): + profile, motion, rows = raw_reference() + attempt = build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved", + session_epoch=2, motion_version=3) + execution = SessionExecution(profile) + execution._effects = [motion] + with pytest.raises(ValueError, match="outside_current_motion"): + execution.record_diagnostic_zero_attempt(attempt) + execution.diagnostic_zero_attempts[motion.zero_joints[0]] = attempt + with pytest.raises(RuntimeError, match="without frozen evidence"): + execution.motion_complete() + with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"): + reject_diagnostic_capture([attempt.as_record()]) + assert not execution.zero_references + + +def test_raw_reference_binding_follows_virtual_task_and_view_names(): + profile, motion, rows = raw_reference() + task_name, view_name = "virtual_arrival", "inspection_camera" + profile = replace(profile, + vision=replace(profile.vision, views=tuple(replace(view, name=view_name) + if view.name == "top" else view for view in profile.vision.views)), + measurement=replace(profile.measurement, measurements={name: + replace(spec, view=view_name) if spec.view == "top" else spec + for name, spec in profile.measurement.measurements.items()}), + motion=replace(profile.motion, tasks=tuple(replace(task, key=task_name, view=view_name) + if task.key == motion.task_key else task for task in reversed(profile.motion.tasks)), + joint_zero_references={name: replace(spec, task_key=task_name) + if spec.task_key == motion.task_key else spec + for name, spec in profile.motion.joint_zero_references.items()})) + motion = replace(motion, task_key=task_name) + for row in rows: + row.update(task_name=task_name, view=view_name) + attempt = build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved", + session_epoch=2, motion_version=3) + assert attempt.task_name == task_name + assert attempt.joint_views == (("rh_thumb_cmc_yaw", view_name),) + assert {view for view, _ in attempt.source_images} == {view_name} + + +def test_diagnostic_reference_cannot_relabel_an_arbitrary_target_as_declared_zero(): + profile, motion, rows = raw_reference() + target = list(motion.target) + target[1] -= 10 + motion = replace(motion, target=tuple(target)) + for row in rows: + row["command_vector"] = target + row["feedback_vector"] = target + with pytest.raises(ValueError, match="declared_pose_mismatch"): + build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved", + session_epoch=2, motion_version=3) + + +@pytest.mark.parametrize("action", ["pause", "abort"]) +def test_late_unresolved_zero_image_cannot_create_diagnostic_attempt(tmp_path, monkeypatch, action): + unresolved_front(monkeypatch) + host, clock = start_diagnostic(tmp_path, monkeypatch) + entered, release = Event(), Event() + try: + run_until(host, clock, lambda: host._motion is not None + and "rh_thumb_ip" in host._motion.zero_joints and host._steady_capture_after_ns is not None) + consume = host.capture.consume + def held(frame, motion, **kwargs): + result = consume(frame, motion, **kwargs) + entered.set() + assert release.wait(5) + return result + monkeypatch.setattr(host.capture, "consume", held) + clock.now += .04 + host.receive_feedback(host.profile.command.names, host.last_command, host.ports.clock_ns()) + with ThreadPoolExecutor(1) as pool: + future = pool.submit(host.receive_detections, projected_frame(host, "front", host.last_command)) + assert entered.wait(2) + if action == "abort": + assert host.abort().success + else: + host._pause("test_pause_during_raw_zero") + before = host.raw_path.read_bytes() + release.set() + future.result(timeout=5) + assert host.raw_path.read_bytes() == before + assert not host.execution.diagnostic_zero_attempts + assert set(host.execution.zero_references) == {"rh_thumb_cmc_yaw"} + assert not (host.parameters.session_dir / "diagnostic_capture.json").exists() + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + release.set() + host.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_report.py b/src/linkerhand_calibration/test/test_diagnostic_report.py new file mode 100644 index 0000000..696219c --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_report.py @@ -0,0 +1,185 @@ +"""Diagnostic summaries preserve evidence without claiming calibration accuracy.""" + +from copy import deepcopy +from dataclasses import replace +import json +from types import SimpleNamespace + +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture +from linkerhand_calibration.runtime.diagnostic_reference import DiagnosticZeroAttempt +from linkerhand_calibration.runtime.diagnostic_report import build_diagnostic_report, fixed_reference_summary, pose_step_summary +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase +from linkerhand_calibration.runtime.status import normalize_status, render_status_zh + + +def pose_row(stamp, angle, *, direction="decreasing", cycle=0, attempt=1, cached=False): + return {"sample_phase": "sweep", "view": "front", "task_name": "thumb_pitch_ip_front", + "cycle": cycle, "direction": direction, "attempt": attempt, "image_stamp_ns": stamp, + "pnp_observation_evidence": {"child": {"tag_role": "thumb_pitch", "tag_id": 1, + "pose_source": "cached_fixed_reference" if cached else "current_image", + "selected_pose": {"quaternion_xyzw": Rotation.from_euler("z", angle, degrees=True).as_quat().tolist()}}}} + + +def test_pose_step_report_deduplicates_tag_images_and_keeps_actual_jump(): + rows = [pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 92)] + original = deepcopy(rows) + report = pose_step_summary(rows + deepcopy(rows)) + tag = report["front:thumb_pitch"] + assert tag["accepted_image_count"] == 2 + assert tag["adjacent_pair_count"] == 1 + assert tag["maximum_rotation_step_deg"] == pytest.approx(92) + assert tag["largest_steps"][0]["interval_ms"] == pytest.approx(33) + assert tag["largest_steps"][0]["to_image"] == "front:1033000000" + assert rows == original + + +@pytest.mark.parametrize("change", [ + {"direction": "increasing"}, {"cycle": 1}, {"attempt": 2}, {"cached": True}, +]) +def test_different_units_or_cached_poses_cannot_create_false_adjacent_jumps(change): + report = pose_step_summary([pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 92, **change)]) + assert report["front:thumb_pitch"]["maximum_rotation_step_deg"] is None + assert not report["front:thumb_pitch"]["largest_steps"] + + +def test_long_gap_is_not_reported_as_a_single_frame_jump(): + report = pose_step_summary([pose_row(1_000_000_000, 0), pose_row(6_000_000_000, 92)]) + assert report["front:thumb_pitch"]["maximum_rotation_step_deg"] is None + + +def test_fixed_reference_reports_image_error_without_claiming_measured_zero_motion(): + rows = [pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 0)] + for index, row in enumerate(rows): + item = row['pnp_observation_evidence']['child'] + item.update(tag_role='front_base', tag_id=0, pose_source='verified_fixed_reference', + candidate_diagnostics={'observation_stamp_ns': row['image_stamp_ns'], + 'pose_constraint': 'fixed_reference', 'fixed_reference_verified': True, + 'fixed_reference_reprojection_error_px': .2 + index*.4}) + rejected = deepcopy(rows[-1]['pnp_observation_evidence']['child']) + rejected.update(selected_pose=None, pose_source='current_image') + rejected['candidate_diagnostics'].update(observation_stamp_ns=1_066_000_000, + fixed_reference_verified=False, fixed_reference_reprojection_error_px=2.0) + events = [{'kind': 'pnp_branch_event', 'view': 'front', 'image_stamp_ns': 1_066_000_000, + 'pnp_observation_evidence': rejected}] + report = fixed_reference_summary(rows + deepcopy(rows) + events) + assert report['front:front_base']['image_count'] == 3 + assert report['front:front_base']['accepted_image_count'] == 2 + assert report['front:front_base']['maximum_reprojection_error_px'] == 2.0 + assert pose_step_summary(rows + events) == {} + + +def test_preset_keeps_profile_immutable_and_rejects_wrong_joint_binding(): + profile = load_bundled_hand_profile("o6_right_8") + before = deepcopy(profile) + plan = resolve_diagnostic_capture(profile, "o6_thumb") + assert plan.task_keys == ("thumb_yaw_top", "thumb_pitch_ip_front") + assert plan.cycles == (0,) + assert profile == before + wrong = replace(profile, motion=replace(profile.motion, tasks=( + replace(profile.motion.tasks[0], command_index=5), *profile.motion.tasks[1:]))) + with pytest.raises(ValueError, match="diagnostic_thumb_task_binding_changed"): + resolve_diagnostic_capture(wrong, "o6_thumb") + + +def test_diagnostic_terminal_and_cycle_count_are_distinct_from_calibration_pass(): + profile = load_bundled_hand_profile("o6_right_8") + plan = resolve_diagnostic_capture(profile, "o6_thumb") + session = CalibrationSession(profile, diagnostic_capture=plan) + session.phase = Phase.SWEEP + status = session.status().as_dict() + status["diagnostic_capture"] = plan.as_dict() + rendered = render_status_zh(normalize_status(status, profile=profile)) + assert "第 1/1 轮" in rendered + assert "诊断扫描" in rendered + assert "不发布标定 JSON/URDF" in rendered + session.phase = Phase.DIAGNOSTIC_COMPLETE + status = normalize_status(session.status().as_dict(), profile=profile) + assert status["state"] == "DIAGNOSTIC_COMPLETE" + assert status["overall_progress_01"] == 1 + assert "未进行精度验收" in render_status_zh(status) + + +def test_diagnostic_cannot_authorize_commit_even_if_publication_phase_is_requested(tmp_path): + from runtime_host_fixture import coordinator_fixture + host, _ = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + try: + host.execution.session.phase = Phase.PUBLISH + with pytest.raises(RuntimeError, match="diagnostic_capture_not_for_publication"): + host._authorize_commit() + assert not host.finalization.started + assert not list(tmp_path.rglob("*.urdf")) + finally: + host.close() + + +def report_inputs(tmp_path): + """Only report binding is under test; no pose/accuracy evidence is synthesized.""" + profile = load_bundled_hand_profile("o6_right_8") + plan = resolve_diagnostic_capture(profile, "o6_thumb") + units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units() + names = ("rh_thumb_cmc_pitch", "rh_thumb_ip") + attempt = DiagnosticZeroAttempt(names, "thumb_pitch_ip_front", "motion_geometry_unresolved", + 1, 11, profile.command.baseline_values, profile.command.baseline_values, + (("front", 1000),), tuple((name, "front") for name in names)) + zero = {"kind": "joint_zero_reference", "joint": "rh_thumb_cmc_yaw"} + records = [zero, json.loads(json.dumps(attempt.as_record()))] + raw_path = tmp_path / "raw_samples.jsonl" + raw_path.write_text("".join(json.dumps(row)+"\n" for row in records)) + return profile, plan, dict(raw_path=raw_path, records=records, + references={"rh_thumb_cmc_yaw": SimpleNamespace(as_record=lambda: zero)}, + completed_units={(unit.task_key, unit.cycle, unit.direction) for unit in units}, + expected_units=units, protected_inputs={}, command=profile.command.baseline_values, + feedback=profile.command.baseline_values, + diagnostic_zero_attempts={name: attempt for name in names}) + + +def test_raw_only_report_has_missing_zeros_without_fabricating_joint_references(tmp_path): + profile, plan, kwargs = report_inputs(tmp_path) + report = build_diagnostic_report(profile, plan, **kwargs) + assert report["missing_joint_zero_references"] == ["rh_thumb_cmc_pitch", "rh_thumb_ip"] + assert set(report["joint_zero_references"]) == {"rh_thumb_cmc_yaw"} + assert report["joint_zero_status"]["rh_thumb_ip"]["status"] == "unresolved" + assert report["joint_zero_status"]["rh_thumb_ip"]["raw_only"] is True + assert report["joint_zero_status"]["rh_thumb_cmc_yaw"]["status"] == "confirmed" + assert report["joint_sample_counts"]["rh_thumb_ip"] == {} + assert report["publication_allowed"] is report["accuracy_validated"] is False + + +def test_report_separates_hold_images_and_samples_from_sweep_counts(tmp_path): + profile, plan, kwargs = report_inputs(tmp_path) + for phase in ("steady", "sweep"): + identity = {"sample_phase": phase, "task_name": "thumb_yaw_top", "view": "top", + "direction": "decreasing", "attempt": 1} + kwargs["records"].append({"kind": "joint_sample", "joint": "rh_thumb_cmc_yaw", **identity}) + kwargs["records"].append({"kind": "diagnostic_observation_frame", **identity}) + kwargs["raw_path"].write_text("".join(json.dumps(row)+"\n" for row in kwargs["records"])) + report = build_diagnostic_report(profile, plan, **kwargs) + key = "top:decreasing:attempt=1" + assert report["joint_sample_counts"]["rh_thumb_cmc_yaw"][key] == 2 # Historical total retained. + assert report["joint_sample_counts_by_phase"]["rh_thumb_cmc_yaw"] == { + "steady": {key: 1}, "sweep": {key: 1}} + assert report["raw_observation_counts_by_phase"] == {"steady": {key: 1}, "sweep": {key: 1}} + + +@pytest.mark.parametrize("fault", ["unrecorded", "overlap", "missing", "binding", "accepted_sample"]) +def test_report_rejects_missing_or_conflicting_unresolved_zero_evidence(tmp_path, fault): + profile, plan, kwargs = report_inputs(tmp_path) + attempts = kwargs["diagnostic_zero_attempts"] + if fault == "unrecorded": + kwargs["records"] = kwargs["records"][:1] + elif fault == "overlap": + kwargs["references"]["rh_thumb_ip"] = kwargs["references"]["rh_thumb_cmc_yaw"] + elif fault == "missing": + del attempts["rh_thumb_ip"] + elif fault == "binding": + attempts["rh_thumb_ip"] = replace(attempts["rh_thumb_ip"], task_name="thumb_yaw_top") + kwargs["records"].append(attempts["rh_thumb_ip"].as_record()) + else: + kwargs["records"].append({"kind": "joint_sample", "joint": "rh_thumb_ip"}) + with pytest.raises(ValueError, match="diagnostic_"): + build_diagnostic_report(profile, plan, **kwargs) diff --git a/src/linkerhand_calibration/test/test_diagnostic_session.py b/src/linkerhand_calibration/test/test_diagnostic_session.py new file mode 100644 index 0000000..1f7490b --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_session.py @@ -0,0 +1,203 @@ +"""Short diagnostic captures reuse the production motion and safety schedule.""" + +from dataclasses import replace + +import pytest + +from linkerhand_calibration.core.domain.reference import build_joint_zero_reference +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.diagnostic_capture import DiagnosticCapturePlan +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.resume import ResumeDecision +from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit, observation_streams +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase, CalibrationSession +from test_joint_zero_runtime import reference_rows + + +THUMB_TASKS = ("thumb_yaw_top", "thumb_pitch_ip_front") +THUMB_JOINTS = {"rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_thumb_ip"} + + +def thumb_capture(): + # Reverse the selection to prove that execution follows the Profile order. + return DiagnosticCapturePlan(mode="o6_thumb", task_keys=tuple(reversed(THUMB_TASKS))) + + +def sweep_rows(profile, unit, *, attempt=1, first_stamp=1): + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + rows = [] + for field, joint, spec in observation_streams(profile, task): + for index in range(129): + rows.append({ + field: joint, "view": spec.view, "task_name": task.key, + "cycle": unit.cycle, "direction": unit.direction, "attempt": attempt, + "image_stamp_ns": first_stamp + len(rows), "sample_phase": "sweep", + "feedback_u8": unit.start + (unit.end - unit.start) * index / 128, + }) + return rows + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def test_formal_schedule_keeps_all_tasks_four_cycles_and_both_directions(layout): + profile = load_bundled_hand_profile(layout) + all_units = CalibrationEngine(profile).scan_units() + units = [unit for unit in all_units if unit.sample_phase == "sweep"] + steady = [unit for unit in all_units if unit.sample_phase == "steady"] + assert len(steady) == 4 * len(profile.motion.tasks) + assert {unit.cycle for unit in steady} == {4, 5} + expected = [] + for task in profile.motion.tasks: + forward = "increasing" if task.end_value > task.start_value else "decreasing" + reverse = "decreasing" if forward == "increasing" else "increasing" + expected.extend((task.key, cycle, direction, start, end) + for cycle in (0, 1, 2, 3) + for direction, start, end in ( + (forward, task.start_value, task.end_value), + (reverse, task.end_value, task.start_value), + )) + assert [(unit.task_key, unit.cycle, unit.direction, unit.start, unit.end) + for unit in units] == expected + assert CalibrationSession(profile).status().task.cycle_count == 4 + + +def test_diagnostic_selection_preserves_profile_order_range_and_speed(): + profile = load_bundled_hand_profile("o6_right_8") + diagnostic = CalibrationEngine(profile, diagnostic_capture=thumb_capture()).scan_units() + formal = CalibrationEngine(profile).scan_units() + assert diagnostic == tuple(unit for unit in formal + if unit.task_key in THUMB_TASKS and unit.cycle == 0) + assert len(diagnostic) == 4 + assert [unit.task_key for unit in diagnostic] == [THUMB_TASKS[0]] * 2 + [THUMB_TASKS[1]] * 2 + + +@pytest.mark.parametrize("failed_attempts", [0, 1, 2]) +def test_thumb_capture_freezes_only_its_zeros_scans_and_returns_without_fitting(failed_attempts): + profile = load_bundled_hand_profile("o6_right_8") + driver = SessionExecution(profile, diagnostic_capture=thumb_capture()) + driver.session.device_ready() + driver.session.start() + command = profile.command.baseline_values + rows, trace, references = [], [], {} + forbidden = {Phase.FIT, Phase.HOLDOUT_VALIDATE, Phase.BUILD_ARTIFACTS, + Phase.VALIDATE_URDF, Phase.PUBLISH, Phase.COMPLETE} + for _ in range(100): + phase = driver.session.phase + assert phase not in forbidden + assert driver.session.status().task.cycle_count == 1 + if phase in {Phase.DIAGNOSTIC_COMPLETE, Phase.PAUSED}: + break + if phase == Phase.REFERENCE_LOCKING: + # Lock all selected tasks' references once, including the front + # view needed by the later task; the unused side is not required. + assert driver.session.status().task.required_tag_ids_by_view == { + "front": (0,), "top": (6,)} + driver.session.reference_locked() + continue + if phase == Phase.EVALUATE: + driver.evaluate(rows) + continue + motion = driver.motion(command) + assert motion is not None, phase + assert motion.phase not in {"steady_prepare", "steady"} + command = motion.target + assert command[2:] == profile.command.baseline_values[2:] + trace.append(motion) + for name in motion.zero_joints: + assert name in THUMB_JOINTS + assert name not in references + reference = build_joint_zero_reference(profile, name, reference_rows(profile, name), + session_epoch=1, motion_version=len(trace)) + references[name] = reference + driver.zero_references[name] = reference + if motion.phase == "sweep": + assert motion.task_key in THUMB_TASKS + assert motion.cycle == 0 + unit = driver.action.scan_unit + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + assert set(task.joints) <= references.keys() + if len([item for item in trace if item.phase == "sweep"]) > failed_attempts: + rows.extend(sweep_rows(profile, unit, attempt=motion.attempt, + first_stamp=len(rows) + 1)) + driver.motion_complete() + else: + pytest.fail("diagnostic executor failed to reach a terminal state") + sweeps = [motion for motion in trace if motion.phase == "sweep"] + assert trace[0].phase == "baseline" + assert driver.session.status().reference_locked + assert references == driver.zero_references + assert max(motion.attempt for motion in sweeps) <= 2 + if failed_attempts: + assert sweeps[0].target == sweeps[1].target + assert sweeps[0].speed == sweeps[1].speed + assert sweeps[1].attempt == 2 + if failed_attempts == 2: + assert driver.session.phase == Phase.PAUSED + assert driver.session.status().pause.code == "sweep_quality_failed" + assert len(sweeps) == 2 + return + assert set(references) == THUMB_JOINTS + assert len(driver.completed_units) == 4 + assert len(sweeps) == 4 + failed_attempts + assert trace[-1].phase == "return" + assert command == profile.command.baseline_values + status = driver.session.status() + assert status.state == status.phase == "DIAGNOSTIC_COMPLETE" + assert status.overall_progress_01 == 1.0 + assert driver.motion(command) is None + with pytest.raises(RuntimeError, match="expected FIT"): + driver.session.fit_complete() + with pytest.raises(RuntimeError, match="expected PUBLISH"): + driver.session.published() + + +def test_diagnostic_sweep_quality_does_not_require_formal_steady_samples(): + profile = load_bundled_hand_profile("o6_right_8") + # Legacy interleaved units require both evidence kinds; diagnostics only + # exercise the sweep. Separate production units have their own gates. + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved")) + unit = CalibrationEngine(profile).scan_units()[0] + rows = sweep_rows(profile, unit) + diagnostic = evaluate_capture_unit(profile, unit, 1, rows, + first_cycle_spans={}, include_steady=False) + formal = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) + assert diagnostic.passed + assert not formal.passed + assert formal.failures and all(reason.startswith("steady:") for reason in formal.failures) + + +def test_diagnostic_capture_refuses_resume_reuse_and_restore_before_mutation(): + profile = load_bundled_hand_profile("o6_right_8") + driver = SessionExecution(profile, diagnostic_capture=thumb_capture()) + driver.session.device_ready() + with pytest.raises(ValueError, match="diagnostic capture cannot resume"): + driver.session.start(resume_requested=True) + assert driver.session.phase == Phase.READY + decision = ResumeDecision(reuse=True, reason="previous", completed_units=( + (THUMB_TASKS[0], 0, "decreasing"),)) + for call in ( + lambda: driver.session.resume_checked(decision), + lambda: driver.authorize_task_reuse(decision.completed_units), + lambda: driver.restore(decision, []), + ): + with pytest.raises(ValueError, match="diagnostic capture cannot"): + call() + assert not driver.completed_units + assert not driver.first_cycle_spans + assert not driver.zero_references + assert driver.session.phase == Phase.READY + + +def test_formal_session_cannot_reuse_a_diagnostic_journal(): + from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION + profile = load_bundled_hand_profile("o6_right_8") + start = {"profile_id": profile.key.profile_id, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION} + formal = CalibrationEngine(profile) + assert formal.resume_compatible(start) + assert not formal.resume_compatible({**start, "diagnostic_capture": thumb_capture().as_dict()}) + diagnostic = CalibrationEngine(profile, diagnostic_capture=thumb_capture()) + assert not diagnostic.resume_compatible(start) + assert not diagnostic.resume_compatible({**start, "diagnostic_capture": thumb_capture().as_dict()}) diff --git a/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py b/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py new file mode 100644 index 0000000..06cb29c --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py @@ -0,0 +1,246 @@ +"""Exercise the short capture through the real CLI and process boundary, offline.""" + +from dataclasses import replace +import importlib.util +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest +import yaml + +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime import runner +from linkerhand_calibration.runtime.ros.parameters import load_runtime_parameters, parameter_defaults +from linkerhand_calibration.runtime.runner_support import launch_command, protected_inputs + + +PACKAGE = Path(__file__).resolve().parents[1] + + +def product(model="o6"): + return load_product_config(PACKAGE / f"config/{model}_right_product.yaml", + workspace=PACKAGE.parents[1], check_can=False) + + +@pytest.mark.parametrize("flag,mode", [ + ("--diagnostic-thumb", "o6_thumb"), ("--diagnostic-yaw-hold", "o6_yaw_hold"), +]) +def test_cli_selects_diagnostic_capture_and_disables_resume(monkeypatch, flag, mode): + calls = [] + config = product() + monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: config) + monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: calls.append(kwargs) or 0) + with pytest.raises(SystemExit) as result: + runner.main([flag, "--record-bag"]) + assert result.value.code == 0 + assert calls == [{"record_bag": True, "commands_enabled": True, + "allow_resume": False, "diagnostic_capture": mode}] + + +@pytest.mark.parametrize("arguments", [ + ["--offline-raw", "old.jsonl"], ["--offline-output", "output"], + ["--publish-offline"], ["--commands-disabled"], +]) +@pytest.mark.parametrize("flag", ["--diagnostic-thumb", "--diagnostic-yaw-hold"]) +def test_cli_rejects_incompatible_modes_before_loading_devices(monkeypatch, arguments, flag): + monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: pytest.fail("loaded hardware config")) + with pytest.raises(SystemExit) as result: + runner.main([flag, *arguments]) + assert result.value.code == 2 + + +@pytest.mark.parametrize("model", ["l6", "o12", "g20"]) +@pytest.mark.parametrize("flag", ["--diagnostic-thumb", "--diagnostic-yaw-hold"]) +def test_thumb_cli_rejects_other_models_without_starting_stack(monkeypatch, model, flag): + config = product(model) + monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: config) + monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: pytest.fail("started stack")) + with pytest.raises(SystemExit) as result: + runner.main([flag, "--validate-only"]) + assert result.value.code == 2 + + +@pytest.mark.parametrize("flag,mode", [ + ("--diagnostic-thumb", "o6_thumb"), ("--diagnostic-yaw-hold", "o6_yaw_hold"), +]) +def test_validate_only_exposes_diagnostic_scope_without_starting_stack(monkeypatch, capsys, flag, mode): + monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: pytest.fail("started stack")) + runner.main(["--config", str(PACKAGE / "config/o6_right_product.yaml"), + "--workspace", str(PACKAGE.parents[1]), flag, "--validate-only"]) + output = capsys.readouterr().out + report, _ = json.JSONDecoder().raw_decode(output) + assert report["diagnostic_capture"]["mode"] == mode + assert report["diagnostic_capture"]["publication_allowed"] is False + if mode == "o6_yaw_hold": + assert report["diagnostic_capture"]["hold_command_values"] == [64., 128., 192.] + assert report["diagnostic_capture"]["hold_seconds"] == 10. + + +def test_cli_diagnostic_presets_are_mutually_exclusive_before_device_loading(monkeypatch): + monkeypatch.setattr(runner, "load_product_config", lambda *a, **kw: pytest.fail("loaded devices")) + with pytest.raises(SystemExit) as result: + runner.main(["--diagnostic-thumb", "--diagnostic-yaw-hold"]) + assert result.value.code == 2 + + +def test_runner_recognizes_diagnostic_completion_as_separate_terminal(): + status = {"state": "DIAGNOSTIC_COMPLETE"} + monitor = SimpleNamespace(status=status) + result = runner.drive_session(monitor, None, spin=lambda: None, ok=lambda: True, + request_start=lambda: pytest.fail("unexpected start")) + assert result == status + + +@pytest.mark.parametrize("terminal,report_change,expected,analysis_outcome", [ + ("DIAGNOSTIC_COMPLETE", {}, 0, "complete"), + ("DIAGNOSTIC_COMPLETE", {}, 0, "failed"), + ("DIAGNOSTIC_COMPLETE", {}, 130, "cancelled"), + ("DIAGNOSTIC_COMPLETE", {"missing_joint_zero_references": ["rh_thumb_ip"], + "accepted_pose_quality_incomplete_attempts": 2}, 0, "complete"), + ("DIAGNOSTIC_COMPLETE", {"mode": "other"}, 2, "complete"), + ("DIAGNOSTIC_COMPLETE", {"publication_allowed": True}, 2, "complete"), + ("DIAGNOSTIC_COMPLETE", {"publication_allowed": 0}, 2, "complete"), + ("DIAGNOSTIC_COMPLETE", {"result": "COMPLETE"}, 2, "complete"), + ("DIAGNOSTIC_COMPLETE", None, 2, "complete"), + ("COMPLETE", {}, 3, "complete"), + ("PAUSED", {}, 3, "complete"), +]) +@pytest.mark.parametrize("diagnostic_mode", ["o6_thumb", "o6_yaw_hold"]) +def test_online_diagnostic_never_enters_artifact_completion( + monkeypatch, tmp_path, capsys, terminal, report_change, expected, analysis_outcome, diagnostic_mode, +): + import rclpy + import linkerhand_calibration.hikrobot_camera as camera + from linkerhand_calibration.runtime import diagnostic_analysis, resume, runner_support + from linkerhand_calibration.runtime.artifacts import completion + + config = replace(product(), output_root=tmp_path) + session = tmp_path / "session" + session.mkdir() + if report_change is not None: + report = {"mode": diagnostic_mode, "result": "DIAGNOSTIC_COMPLETE", + "publication_allowed": False, **report_change} + (session / "diagnostic_capture.json").write_text(json.dumps(report)) + now = [0.0] + launched = [] + stopped = [] + analyzed = [] + monitor = SimpleNamespace( + count_publishers=lambda topic: 0, + status_topic="/offline/status", + destroy_node=lambda: stopped.append("monitor"), + ) + monkeypatch.setattr(rclpy, "ok", lambda: True) + monkeypatch.setattr(rclpy, "spin_once", lambda *args, **kwargs: None) + + def clock(): + now[0] += 1.0 + return now[0] + + monkeypatch.setattr(runner.time, "monotonic", clock) + monkeypatch.setattr(runner_support, "create_session_directory", lambda root: session) + monkeypatch.setattr(runner_support, "CalibrationMonitor", lambda **kwargs: monitor) + monkeypatch.setattr(runner_support, "stop_stack", lambda process: stopped.append("stack")) + def analyze_after_shutdown(profile, directory): + assert stopped == ["stack"] + assert directory == session + analyzed.append(profile.key.profile_id) + if analysis_outcome == "cancelled": + raise KeyboardInterrupt + return {"status": analysis_outcome, "accuracy_validated": False, "publication_allowed": False} + monkeypatch.setattr(diagnostic_analysis, "finish_diagnostic_analysis", analyze_after_shutdown) + monkeypatch.setattr(camera, "configure_fastdds_large_image_transport", lambda: None) + monkeypatch.setattr(resume, "discover_resume_candidate", + lambda *args, **kwargs: pytest.fail("diagnostic reused old samples")) + monkeypatch.setattr(completion, "finish_online_artifacts", + lambda *args, **kwargs: pytest.fail("diagnostic attempted publication")) + monkeypatch.setattr(runner.subprocess, "Popen", lambda command, **kwargs: launched.append(command) or object()) + monkeypatch.setattr(runner, "drive_session", lambda *args, **kwargs: {"state": terminal}) + + assert runner.run_online(config, diagnostic_capture=diagnostic_mode) == expected + assert len(launched) == 1 + assert f"diagnostic_capture:={diagnostic_mode}" in launched[0] + assert not any(arg.startswith("resume_raw_samples_path:=") for arg in launched[0]) + assert stopped == ["stack", "monitor"] + assert analyzed == ([config.profile_key.profile_id] if expected in {0, 130} else []) + if expected in {0, 130}: + output = capsys.readouterr().out + assert "诊断数据已采完" in output + assert "本次未进行精度验收，未发布 JSON 或修正 URDF" in output + if diagnostic_mode == "o6_yaw_hold": + assert "拇指yaw方向差保持诊断" in output + if report_change.get("missing_joint_zero_references"): + assert "零位未确认，仅保留原始观测：rh_thumb_ip" in output + assert "未生成这些关节的零位或角度样本" in output + assert "2 次方向采集的可信姿态覆盖不足" in output + if expected == 130: + assert "诊断采集已完成；已取消后续分析" in output + assert "已请求中止" not in output + assert not (session / "runner_diagnostic.json").exists() + else: + assert (session / "runner_diagnostic.json").is_file() + + +def runtime_values(config, tmp_path): + profile = config.calibration_contract.typed_profile + declared = next(iter(yaml.safe_load(config.calibration_config.read_text()).values()))["ros__parameters"] + return {**parameter_defaults(profile), **declared, + "serial_number": "OFFLINE_DIAGNOSTIC", "session_dir": str(tmp_path), + "source_urdf_path": str(config.source_urdf), + "camera_extrinsics_file": str(config.camera_extrinsics), + **{key.replace("_sha256", "_expected_sha256"): value + for key, value in protected_inputs(config).items()}} + + +@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) +def test_ros_parameters_load_explicit_diagnostic_mode(tmp_path, mode): + config = product() + values = runtime_values(config, tmp_path) + assert values["diagnostic_capture"] == "" + values["diagnostic_capture"] = mode + resolved = load_runtime_parameters(config.calibration_contract.typed_profile, values.__getitem__) + assert resolved.diagnostic_capture == mode + assert resolved.resume_raw_samples_path is None + + +@pytest.mark.parametrize("extra", [ + {"commands_enabled": False}, {"resume_raw_samples_path": "old.jsonl"}, + {"diagnostic_capture": "unknown"}, +]) +@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) +def test_direct_ros_entry_cannot_bypass_diagnostic_constraints(tmp_path, extra, mode): + config = product() + values = {**runtime_values(config, tmp_path), "diagnostic_capture": mode, **extra} + with pytest.raises(ValueError): + load_runtime_parameters(config.calibration_contract.typed_profile, values.__getitem__) + + +@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) +def test_launch_forwards_diagnostic_string_to_owner_and_suppresses_resume(tmp_path, monkeypatch, mode): + from launch import LaunchContext + from launch.actions import DeclareLaunchArgument + from launch_ros.actions import Node + from launch_ros.utilities import evaluate_parameters + from ros2launch.api.api import parse_launch_arguments + + monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) + config = product() + arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", + record_bag=False, commands_enabled=True, resume_from=tmp_path / "old", + diagnostic_capture=mode)[4:])) + assert "resume_raw_samples_path" not in arguments + assert arguments["diagnostic_capture"] == mode + spec = importlib.util.spec_from_file_location("diagnostic_launch", + PACKAGE / "launch/three_camera_calibration.launch.py") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + context = LaunchContext() + context.launch_configurations.update(arguments) + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + nodes = [action for action in module._launch_stack(context) if isinstance(action, Node)] + owner = next(node for node in nodes if node.node_executable == "three_camera_calibration_node") + resolved = evaluate_parameters(context, owner._Node__parameters) + assert any(isinstance(item, dict) and item.get("diagnostic_capture") == mode for item in resolved) diff --git a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py new file mode 100644 index 0000000..a9bb1fc --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py @@ -0,0 +1,139 @@ +"""The real coordinator holds six declared poses without publishing artifacts. + +Projected image inputs exercise the public geometry/capture path. This test +checks schedule duration, raw observation gates and publication rules; its +synthetic Tags do not constitute a hardware accuracy certificate. +""" + +from collections import defaultdict +import hashlib +import json + +import numpy as np +import pytest + +from linkerhand_calibration.acquisition import StateSample +from linkerhand_calibration.runtime.diagnostic_capture import diagnostic_hold_timing +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready +from test_diagnostic_coordinator import advance, assert_terminal_is_idle, TERMINAL_PHASES + + +def test_hold_timing_uses_only_this_segment_commands_at_or_before_image(): + target = (255., 192.) + history = [StateSample(90, target), StateSample(100, (255., 220.)), + StateSample(115, target), StateSample(120, target), StateSample(140, target)] + early = diagnostic_hold_timing(history, target=target, image_stamp_ns=110, + session_epoch=2, motion_version=7, motion_started_ns=100, feedback_stable_since_ns=None) + assert early == {"hold_id": "2:7", "target_first_command_stamp_ns": None, + "feedback_stable_since_ns": None} + current = diagnostic_hold_timing(history, target=target, image_stamp_ns=125, + session_epoch=2, motion_version=7, motion_started_ns=100, feedback_stable_since_ns=120) + assert current["target_first_command_stamp_ns"] == 115 + assert current["feedback_stable_since_ns"] == 120 + next_segment = diagnostic_hold_timing(history, target=target, image_stamp_ns=145, + session_epoch=2, motion_version=8, motion_started_ns=130, feedback_stable_since_ns=None) + assert next_segment["hold_id"] == "2:8" and next_segment["target_first_command_stamp_ns"] == 140 + + +@pytest.mark.parametrize("stable_stamp", [90, 126]) +def test_hold_timing_does_not_import_stability_from_previous_segment_or_future(stable_stamp): + result = diagnostic_hold_timing([StateSample(110, (255., 192.))], target=(255., 192.), + image_stamp_ns=125, session_epoch=2, motion_version=7, motion_started_ns=100, + feedback_stable_since_ns=stable_stamp) + assert result["target_first_command_stamp_ns"] == 110 + assert result["feedback_stable_since_ns"] is None + + +def test_yaw_hold_completes_six_full_holds_and_keeps_pre_stable_raw_images(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_yaw_hold") + ready(host, clock) + assert set(host.required_observation_views) == {"top"} + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + monkeypatch.setattr(host.finalization, "start", lambda *a, **kw: pytest.fail("diagnostic started finalizer")) + assert host.start().success + intervals, order, cursor, checked_three = {}, [], 0, set() + accepted = defaultdict(int) + previous = None + try: + for _ in range(9000): + if host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE: + break + if host.execution.session.phase in TERMINAL_PHASES: + pytest.fail(f"premature diagnostic exit: {host.state}: {host.reason}") + advance(host, clock) + motion = host._motion + key = None + if motion is not None and motion.phase == "steady": + key = (motion.direction, motion.steady_index, motion.target[motion.command_index]) + if key not in intervals: + order.append(key) + intervals[key] = {"start": clock.now, "target_first": None, "end": None} + if tuple(host.last_command) == motion.target and intervals[key]["target_first"] is None: + intervals[key]["target_first"] = clock.now + if previous is not None and previous != key: + intervals[previous]["end"] = clock.now + previous = key + for row in host.raw_records[cursor:]: + if row.get("kind") != "joint_sample" or row.get("sample_phase") != "steady": + continue + sample_key = (row["direction"], row["steady_index"], row["steady_target"]) + accepted[sample_key] += 1 + if accepted[sample_key] == 3: + # Three good frames must not terminate a ten-second hold. + assert key == sample_key + assert host.segment is not None and not host.segment.minimum_hold_complete(clock.now) + checked_three.add(sample_key) + cursor = len(host.raw_records) + else: + pytest.fail(f"virtual yaw-hold capture timed out: {host.state}: {host.reason}") + assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE + expected = [("decreasing", i, value) for i, value in enumerate((192., 128., 64.))] + expected += [("increasing", i, value) for i, value in enumerate((64., 128., 192.))] + assert order == expected + assert checked_three == set(expected) + for key in expected: + interval = intervals[key] + assert interval["target_first"] is not None and interval["end"] is not None + assert interval["end"] - interval["target_first"] >= 10. - 1e-9 + assert accepted[key] > 3 + + holds = defaultdict(list) + for row in host.raw_records: + if row.get("kind") == "diagnostic_observation_frame" and row.get("sample_phase") == "steady": + holds[(row["direction"], row["steady_index"], row["steady_target"])].append(row) + assert set(holds) == set(expected) + hold_ids, versions = set(), set() + for key, rows in holds.items(): + assert {row["minimum_hold_seconds"] for row in rows} == {10.} + assert any(not row["accepted_sample_window_open"] for row in rows) + assert any(row["accepted_sample_window_open"] for row in rows) + assert all(row["is_accepted_joint_sample"] is False for row in rows) + assert all(row["command_vector"] is not None and row["feedback_vector"] is not None for row in rows) + assert len({row["hold_id"] for row in rows}) == 1 + assert len({row["session_epoch"] for row in rows}) == 1 + assert len({row["motion_version"] for row in rows}) == 1 + assert len({row["motion_started_ns"] for row in rows}) == 1 + hold_ids.add(rows[0]["hold_id"]) + versions.add(rows[0]["motion_version"]) + assert all(row["image_stamp_ns"] >= row["motion_started_ns"] for row in rows) + target_stamps = {row["target_first_command_stamp_ns"] for row in rows + if row["target_first_command_stamp_ns"] is not None} + assert len(target_stamps) == 1 + assert max(row["image_stamp_ns"] for row in rows) - next(iter(target_stamps)) >= 9.9e9 + assert any(row["feedback_stable_since_ns"] is None for row in rows) + assert any(row["feedback_stable_since_ns"] is not None for row in rows) + assert len(hold_ids) == len(versions) == 6 + references = [row for row in host.raw_records if row.get("kind") == "joint_zero_reference"] + assert [row["joint"] for row in references] == ["rh_thumb_cmc_yaw"] + assert len(host.execution.completed_units) == 2 + assert not host.finalization.started and not host.final_json and not host.final_urdf + assert not list(tmp_path.rglob("*.urdf")) + assert all(tuple(command[i] for i in (0, 2, 3, 4, 5)) == (255.,) * 5 for command in clock.positions) + report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text()) + assert not report["publication_allowed"] and not report["accuracy_validated"] + assert report["raw_samples_sha256"] == hashlib.sha256(host.raw_path.read_bytes()).hexdigest() + assert_terminal_is_idle(host, clock, monkeypatch) + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py new file mode 100644 index 0000000..25077c1 --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py @@ -0,0 +1,59 @@ +"""The directional-hold preset has bounded motion and diagnostic-only scope.""" + +from dataclasses import replace + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.diagnostic_capture import ( + DiagnosticCapturePlan, resolve_diagnostic_capture, +) +from linkerhand_calibration.runtime.observation_scope import required_observation_views +from linkerhand_calibration.runtime.status import render_status_zh + + +def test_yaw_hold_scope_and_legacy_serialization(): + profile = load_bundled_hand_profile("o6_right_8") + plan = resolve_diagnostic_capture(profile, "o6_yaw_hold") + assert plan.as_dict() == {"mode": "o6_yaw_hold", "task_keys": ("thumb_yaw_top",), + "cycles": (0,), "hold_command_values": (64., 128., 192.), "hold_seconds": 10.} + assert required_observation_views(profile, plan) == ("top",) + legacy = resolve_diagnostic_capture(profile, "o6_thumb") + assert legacy.hold_command_values == () and legacy.hold_seconds == 0. + assert legacy.as_dict() == {"mode": "o6_thumb", "cycles": (0,), + "task_keys": ("thumb_yaw_top", "thumb_pitch_ip_front")} + text = render_status_zh({"phase": "steady", "diagnostic_capture": plan.as_dict()}) + assert "拇指yaw方向差保持诊断" in text + assert "方向差保持采集（每点至少 10 秒）" in text + assert "不发布标定 JSON/URDF" in text + complete = render_status_zh({"phase": "DIAGNOSTIC_COMPLETE", "diagnostic_capture": plan.as_dict()}) + assert "诊断采集完成（未进行精度验收）" in complete + + +@pytest.mark.parametrize("values,seconds", [ + ((64., 128.), 0.), ((64.,), 10.01), ((64.,), float("nan")), + ((), 10.), ((64., float("inf")), 10.), ((64., 64.), 10.), + ((128., 64.), 10.), (("64",), 10.), +]) +def test_invalid_hold_contract_is_rejected(values, seconds): + with pytest.raises(ValueError, match="diagnostic_hold_"): + DiagnosticCapturePlan("invalid", ("task",), hold_command_values=values, hold_seconds=seconds) + + +@pytest.mark.parametrize("change", ["scan_endpoint", "channel_envelope", "joint_binding"]) +def test_yaw_hold_refuses_changed_profile_motion_contract(change): + profile = load_bundled_hand_profile("o6_right_8") + task = next(task for task in profile.motion.tasks if task.key == "thumb_yaw_top") + if change == "scan_endpoint": + tasks = tuple(replace(item, end_u8=64) if item == task else item for item in profile.motion.tasks) + profile = replace(profile, motion=replace(profile.motion, tasks=tasks)) + elif change == "channel_envelope": + upper = list(profile.command.maximum_values) + upper[task.command_index] = 192. + profile = replace(profile, command=replace(profile.command, upper_bounds=tuple(upper))) + else: + bindings = dict(profile.command.command_index_by_joint) + bindings["rh_thumb_cmc_yaw"] = (task.command_index + 1) % profile.command.command_count + profile = replace(profile, command=replace(profile.command, command_index_by_joint=bindings)) + with pytest.raises(ValueError, match="diagnostic_(hold_outside_task_domain|thumb_task_binding_changed)"): + resolve_diagnostic_capture(profile, "o6_yaw_hold") diff --git a/src/linkerhand_calibration/test/test_directional_command_mapping.py b/src/linkerhand_calibration/test/test_directional_command_mapping.py new file mode 100644 index 0000000..0a81708 --- /dev/null +++ b/src/linkerhand_calibration/test/test_directional_command_mapping.py @@ -0,0 +1,156 @@ +"""Recorded major-loop hysteresis cannot be hidden by a single command table.""" +from copy import deepcopy +from dataclasses import replace +import gzip +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.reference import read_joint_zero_references +from linkerhand_calibration.core.domain.result import CalibrationResult, JointMapping +from linkerhand_calibration.core.fitting.observed_motion import fit_native_observed_rotation +from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings +from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping +from linkerhand_calibration.core.urdf.measured_acceptance import validate_measured_joint_holdout +from linkerhand_calibration.runtime.acquisition import accepted_joint_records +from linkerhand_calibration.runtime.artifacts.direction import next_input_directions +from linkerhand_calibration.runtime.artifacts.reader import UnifiedCommandMapper +from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import serialize_report, from_report + +JOINT = "rh_thumb_cmc_yaw" + + +@pytest.fixture(scope="module") +def recorded(): + payload = json.loads(gzip.decompress((Path(__file__).parent / "fixtures" / + "o6_yaw_directional_114156.json.gz").read_bytes())) + profile = load_bundled_hand_profile("o6_right_8") + # This recording used nine training nodes; later profiles may sample more. + profile = replace(profile, acquisition=replace(profile.acquisition, steady_training_nodes=9, + command_capture_mode="interleaved", steady_extra_training_nodes={})) + profile = replace(profile, measurement=replace(profile.measurement, + measurements={JOINT: profile.measurement.measurements[JOINT]}, + transferred_motion_sources={}, cross_view_sources={})) + rows = payload["records"] + reference = read_joint_zero_references(profile, rows)[JOINT] + sweep = accepted_joint_records(profile, rows)[JOINT] + steady = accepted_joint_records(profile, rows, sample_phase="steady") + curve, errors = fit_native_observed_rotation(sweep, input_domain="feedback_u8", + input_to_joint_direction=-1, reference_xyzw=reference.quaternion("top")) + mapping = JointMapping(JOINT, 1, "feedback_u8", tuple(curve.circle["input_knots"]), + curve.angle_rad, curve.increasing_rad, curve.decreasing_rad) + fit = CalibrationResult({JOINT: curve}, {JOINT: mapping}, {JOINT: 0.}, + {JOINT: "test_only_geometry"}, {}, {JOINT: errors}, None, + reference_inputs={JOINT: reference.input_reference("top", feedback=True)}, + joint_zero_references={JOINT: reference}) + return profile, fit, steady + + +def directional_payload(recorded): + profile, fit, steady = recorded + fitted = fit_command_mappings(profile, fit, steady) + prepared = SimpleNamespace(mappings=fitted.output_mappings, plan=SimpleNamespace(mimic_output={})) + report = serialize_report(profile, fitted, prepared, serial_number="TEST_ONLY", + protected_inputs={}) + return from_report(report), fitted, report + + +def test_recorded_single_table_fails_both_serialized_directional_branches_pass(recorded): + profile, fit, steady = recorded + single = replace(profile, artifacts=replace(profile.artifacts, directional_command_joints=frozenset())) + with pytest.raises(ValueError, match="steady_command_holdout_failed"): + fit_command_mappings(single, fit, steady) + payload, fitted, _ = directional_payload(recorded) + validation = [row for row in steady[JOINT] if row["cycle"] == 3] + metrics = validate_measured_joint_holdout(payload, fit.curves, validation) + for direction, value in metrics[JOINT].items(): + assert value["mae_deg"] < .3 + assert value["p95_deg"] < 2 + assert value["maximum_deg"] < 3 + mapping = SerializedJointMapping(payload) + baseline = profile.command.baseline_values + for direction in ("", "increasing", "decreasing"): + assert mapping.evaluate(baseline, direction, active_joints=(JOINT,))[JOINT] == 0 + values = list(baseline); values[1] = 128 + with pytest.raises(ValueError, match="requires_direction"): + mapping.evaluate(values, "", active_joints=(JOINT,)) + angles = [mapping.evaluate(values, d, active_joints=(JOINT,))[JOINT] + for d in ("increasing", "decreasing")] + assert np.degrees(abs(angles[0]-angles[1])) > 2 + assert fitted.command_applicability[JOINT]["interior_reversals_validated"] is False + + +def test_bad_branch_or_direction_declaration_cannot_pass(recorded): + payload, fit, _ = directional_payload(recorded) + bad = deepcopy(payload) + bad["joints"][JOINT]["increasing_rad"] = bad["joints"][JOINT]["decreasing_rad"] + with pytest.raises(ValueError, match="serialized_measured_mapping_inaccurate"): + validate_measured_joint_holdout(bad, fit.curves, + [r for r in recorded[2][JOINT] if r["cycle"] == 3]) + for mutation in ("missing", "undeclared", "unknown"): + bad = deepcopy(payload) + if mutation == "missing": del bad["joints"][JOINT]["decreasing_rad"] + if mutation == "undeclared": del bad["directional_command_joints"] + if mutation == "unknown": bad["directional_command_joints"].append("unknown") + with pytest.raises(ValueError): SerializedJointMapping(bad) + + +def test_runtime_direction_initialization_hold_endpoint_turn_and_interior_reversal(): + channels = {0: (255., 0., 255.)} + advance = lambda value, previous, direction: next_input_directions([value], + None if previous is None else [previous], [direction], resolution=.5, channels=channels) + with pytest.raises(ValueError, match="baseline_initialization"): advance(128, None, "") + assert advance(255, None, "") == ["increasing"] + assert advance(128, 255, "increasing") == ["decreasing"] + assert advance(128, 128, "decreasing") == ["decreasing"] + assert advance(128, 0, "decreasing") == ["increasing"] + with pytest.raises(ValueError, match="interior_reversal"): advance(140, 128, "decreasing") + + +def test_reader_uses_recorded_branches_and_commits_state_only_on_success(recorded, tmp_path): + payload, _, report = directional_payload(recorded) + urdf = tmp_path / "reader_only.urdf" + urdf.write_text(f''' + + + + + ''') + mapper = UnifiedCommandMapper(payload, urdf, report=report) + values = list(recorded[0].command.baseline_values) + values[1] = 128 + with pytest.raises(ValueError, match="baseline_initialization"): + mapper.map_positions(values) + assert mapper.previous is None + values[1] = 255 + assert mapper.map_positions(values) == (0.,) + values[1] = 254.6 + assert mapper.map_positions(values) == (0.,) + values[1] = 254.2 + assert mapper.map_positions(values) == (payload["joints"][JOINT]["decreasing_rad"][254],) + assert mapper.directions[1] == "decreasing" + values[1] = 128 + descending = mapper.map_positions(values) + assert descending == (payload["joints"][JOINT]["decreasing_rad"][128],) + assert mapper.map_positions(values) == descending + values[1] = 128.4 + assert mapper.map_positions(values) == descending + values[1] = 128.8 + with pytest.raises(ValueError, match="interior_reversal"): + mapper.map_positions(values) + assert mapper.previous[1] == 128.4 + values[1] = 140 + with pytest.raises(ValueError, match="interior_reversal"): + mapper.map_positions(values) + assert mapper.previous[1] == 128.4 + assert mapper.directions[1] == "decreasing" + values[1] = 0 + mapper.map_positions(values) + values[1] = 128 + ascending = mapper.map_positions(values) + assert ascending == (payload["joints"][JOINT]["increasing_rad"][128],) + assert np.degrees(abs(ascending[0]-descending[0])) > 2 diff --git a/src/linkerhand_calibration/test/test_dual_mapping.py b/src/linkerhand_calibration/test/test_dual_mapping.py index 0b63b5d..bbdbf0d 100644 --- a/src/linkerhand_calibration/test/test_dual_mapping.py +++ b/src/linkerhand_calibration/test/test_dual_mapping.py @@ -1,5 +1,6 @@ """Command calibration uses visual truth, never feedback relabelled as command.""" +from dataclasses import replace import numpy as np import pytest from scipy.spatial.transform import Rotation @@ -17,6 +18,8 @@ from test_joint_zero_runtime import reference_rows def independent_steady_fixture(layout): profile = load_bundled_hand_profile(layout) + # Explicit legacy partitions: the negative cases alter cycles 1 and 3. + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved")) task = profile.motion.tasks[0] name = task.joints[0] channel = task.command_index diff --git a/src/linkerhand_calibration/test/test_feedback_capture_timing.py b/src/linkerhand_calibration/test/test_feedback_capture_timing.py new file mode 100644 index 0000000..549978e --- /dev/null +++ b/src/linkerhand_calibration/test/test_feedback_capture_timing.py @@ -0,0 +1,37 @@ +"""Timer starvation cannot discard independently received SDK observations.""" + +import pytest + +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution +from runtime_host_fixture import coordinator_fixture + + +@pytest.mark.parametrize("pulse", [False, True]) +def test_sdk_feedback_between_slow_ticks_defines_the_stable_window(tmp_path, pulse): + host, clock = coordinator_fixture(tmp_path) + try: + profile = host.profile + baseline = profile.command.baseline_values + command = MotionCommand("joint_zero", baseline, 40., command_index=1) + segment = MotionExecution(profile, command, initial_command=baseline, + initial_feedback=baseline, now=clock.now, identity="slow_timer") + host.segment = segment + segment.sample(clock.now) + start = clock.now + for index in range(16): + clock.now = start+index*.02 + feedback = list(baseline) + if pulse and index == 9: + feedback[0] -= 5 # A held channel moves between control ticks. + host.receive_feedback(profile.command.names, feedback, host.ports.clock_ns()) + if index in {0, 15}: + segment.sample(clock.now) + segment.observe(host.latest_feedback, stamp=host.state_history[-1].stamp_ns, now=clock.now) + assert len(segment.stamped_history) == 16 + assert segment.steady_ready(clock.now) is not pulse + # Replayed feedback identities do not manufacture a larger sample set. + before = len(segment.stamped_history) + host.receive_feedback(profile.command.names, baseline, host.ports.clock_ns()) + assert len(segment.stamped_history) == before + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_final_image_holdout.py b/src/linkerhand_calibration/test/test_final_image_holdout.py new file mode 100644 index 0000000..6600365 --- /dev/null +++ b/src/linkerhand_calibration/test/test_final_image_holdout.py @@ -0,0 +1,85 @@ +"""Final pixels are independent of the intermediate reconstructed Tag poses.""" + +from dataclasses import replace +import json + +import cv2 +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points +from linkerhand_calibration.core.urdf.image_acceptance import TagImageObservation, validate_serialized_image_holdout +from linkerhand_calibration.runtime.artifacts.publisher import FrozenTagArtifactValidator +from test_frozen_tag_replay import fixture + + +def independent_pixels(observations): + camera = ((1200., 0., 640.), (0., 1200., 480.), (0., 0., 1.)) + # Distinct camera view; the generator starts from independent physical poses. + view = np.eye(4) + view[:3, :3] = Rotation.from_euler("xyz", (.08, -.1, .04)).as_matrix() + view[:3, 3] = (.02, -.03, .5) + images = [] + for row in observations: + pose = view @ np.asarray(row.common_from_tag) + corners, _ = cv2.projectPoints(square_object_points(.016), Rotation.from_matrix(pose[:3, :3]).as_rotvec(), + pose[:3, 3], np.asarray(camera), np.zeros(5)) + images.append(TagImageObservation(row.sample_id, row.role, row.cycle, row.sdk_values, + tuple(sorted(row.sdk_directions_by_joint.items())), "independent_view", .016, + tuple(map(tuple, corners.reshape(4, 2))), camera, tuple(map(tuple, view)))) + return tuple(images) + + +@pytest.fixture +def scene(tmp_path): + source, plan, base, _, mounts, holdout, payload = fixture(tmp_path) + written = plan.write(source, tmp_path / "corrected.urdf") + images = independent_pixels(holdout) + kwargs = dict(corrected_urdf=written, payload=payload, common_from_base=base, installations=mounts, + observations=images, independent_observations=holdout, + required_roles=("a", "b"), input_kind="feedback", holdout_cycle=3, minimum_samples=40) + return source, plan, holdout, kwargs + + +def test_written_json_urdf_predict_original_corners_without_pose_refitting(scene): + *_, kwargs = scene + report = validate_serialized_image_holdout(**kwargs) + assert set(report) == {"a", "b"} + assert max(value["maximum_px"] for value in report.values()) < 1e-6 + + +@pytest.mark.parametrize("mutation", ["pixels", "camera", "extrinsics", "size", "overlap", "duplicate", "missing"]) +def test_wrong_raw_pixels_or_camera_cannot_hide_behind_good_3d_poses(scene, mutation): + *_, kwargs = scene + images = kwargs["observations"] + row = images[0] + if mutation == "pixels": + row = replace(row, corners_xy=tuple((x+8, y) for x, y in row.corners_xy)) + elif mutation == "camera": + camera = np.asarray(row.camera_matrix).copy(); camera[0, 0] *= 1.2 + row = replace(row, camera_matrix=tuple(map(tuple, camera))) + elif mutation == "extrinsics": + camera = np.asarray(row.view_from_common).copy(); camera[0, 3] += .01 + row = replace(row, view_from_common=tuple(map(tuple, camera))) + elif mutation == "size": + row = replace(row, tag_size_m=.03) + elif mutation == "overlap": + row = replace(row, cycle=0) + elif mutation == "duplicate": + images += (row,) + elif mutation == "missing": + images = tuple(item for item in images if item.role == "a") + with pytest.raises(ValueError, match="final_image_holdout"): + validate_serialized_image_holdout(**{**kwargs, "observations": (row, *images[1:])}) + + +def test_publisher_requires_pixels_after_3d_passes(scene, tmp_path): + source, plan, holdout, kwargs = scene + calibration = tmp_path / "calibration.json" + calibration.write_text(json.dumps(kwargs["payload"])) + validator = FrozenTagArtifactValidator(kwargs["payload"]["profile_id"], source, plan.source_sha256, + plan.authorized_fields, plan.zero_offsets_rad, kwargs["common_from_base"], kwargs["installations"], + tuple(holdout), ("a", "b"), lambda _: None, require_image_holdout=True) + with pytest.raises(ValueError, match="final_image_holdout_missing"): + validator(calibration, kwargs["corrected_urdf"]) diff --git a/src/linkerhand_calibration/test/test_final_image_sources.py b/src/linkerhand_calibration/test/test_final_image_sources.py new file mode 100644 index 0000000..23f6137 --- /dev/null +++ b/src/linkerhand_calibration/test/test_final_image_sources.py @@ -0,0 +1,55 @@ +"""Actual camera metadata/corners bind to the configured protected file.""" + +from copy import deepcopy +import gzip +import json +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.geometry.extrinsics import transform_matrix +from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime.artifacts.image_evidence import prepare_image_holdout + + +def scene(): + config = load_product_config(Path(__file__).resolve().parents[1]/"config/o6_right_product.yaml", check_can=False) + raw = json.loads(gzip.decompress((Path(__file__).parent/"fixtures/o6_final_image_sources_160040.json.gz").read_bytes())) + assert raw["source_raw_sha256"] == "57ebe3d1987e14ca48911c8db1dddb680133d3edf436e59da7677bbf6931a236" + profile = config.calibration_contract.typed_profile + observations = [] + for row in raw["samples"]: + role = profile.measurement.measurements[row["joint"]].child_role + pose = row["child_pose_common"] + observations.append(TagHoldout(f"{row['view']}:{row['image_stamp_ns']}", role, 3, + tuple(row["state_u8"]), {}, tuple(map(tuple, transform_matrix(pose["translation_xyz_m"], pose["quaternion_xyzw"]))))) + return config, [*raw["models"], *raw["frames"]], observations + + +def test_raw_pixels_bind_without_using_intermediate_tag_poses(): + config, records, observations = scene() + images = prepare_image_holdout(config.calibration_contract.typed_profile, records, observations, + camera_extrinsics_file=config.camera_extrinsics, + protected_inputs={"camera_extrinsics_sha256": config.camera_extrinsics_sha256}) + assert len(images) == len(observations) == 2 + assert {image.view for image in images} == {"top", "front"} + assert all(len(image.corners_xy) == 4 for image in images) + + +@pytest.mark.parametrize("mutation", ["missing", "intrinsics", "matrix", "size", "duplicate", "file_hash"]) +def test_missing_or_changed_camera_identity_cannot_enter_final_check(mutation): + config, records, observations = scene() + records = deepcopy(records) + matrix = next(row for row in records if row.get("kind") == "rectified_camera_model" and row["view"] == "top") + frame = next(row for row in records if row.get("kind") == "pnp_candidate_frame" and row["view"] == "top") + digest = config.camera_extrinsics_sha256 + if mutation == "missing": records.remove(frame) + elif mutation == "intrinsics": matrix["raw_k"][0] += 10 + elif mutation == "matrix": frame["camera_matrix"][0][0] += 10 + elif mutation == "size": next(iter(frame["roles"].values()))["tag_size_m"] = .02 + elif mutation == "duplicate": records.append({**frame, "camera_matrix": [[1, 0, 0], [0, 1, 0], [0, 0, 1]]}) + else: digest = "f"*64 + with pytest.raises(ValueError, match="final_image"): + prepare_image_holdout(config.calibration_contract.typed_profile, records, observations, + camera_extrinsics_file=config.camera_extrinsics, protected_inputs={"camera_extrinsics_sha256": digest}) diff --git a/src/linkerhand_calibration/test/test_fixed_reference_pose.py b/src/linkerhand_calibration/test/test_fixed_reference_pose.py new file mode 100644 index 0000000..e927bbd --- /dev/null +++ b/src/linkerhand_calibration/test/test_fixed_reference_pose.py @@ -0,0 +1,234 @@ +"""Stationarity is checked against live corners, not changing IPPE rankings.""" + +from concurrent.futures import ThreadPoolExecutor +from dataclasses import replace +import json +from pathlib import Path +from threading import Event + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.pnp import SquareTagPose, SquareTagPoseTracker +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.core.geometry.tag_pose.branch_confirmation import BranchState +from linkerhand_calibration.core.geometry.tag_pose.fixed_reference import FixedTagReference +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.pose_evidence import reference_branch_revisions +from test_observation_capture import frame, make_capture + + +REAL = json.loads((Path(__file__).parent / 'fixtures' / + 'o6_front_base_20260914_branch_events.json').read_text()) +ROLE = REAL['tag_role'] +STEP_NS = 40_000_000 + + +def recorded_reference(): + pose = REAL['reference_pose'] + return FixedTagReference(SquareTagPose(tuple(pose['rotation_xyzw']), + tuple(pose['translation_xyz_m']), 0.), 1) + + +def seeded_tracker(reference, stamp): + # Successful preparation frames were not logged. Start at the recorded + # frozen reference boundary; this is explicitly not an end-to-end replay. + tracker = SquareTagPoseTracker() + tracker._branches.states[ROLE] = BranchState(observed_ns=stamp, pose=reference.pose, + revision=reference.branch_revision, frozen=True, status='tracking', reason='') + return tracker + + +def observe(tracker, stamp, corners, reference): + return tracker.estimate(ROLE, corners, tag_size_m=REAL['tag_size_m'], + camera_matrix=REAL['camera_matrix'], stamp_ns=stamp, fixed_reference=reference) + + +def test_recorded_id0_conflict_is_resolved_only_with_declared_stationarity(): + reference = recorded_reference() + first = REAL['events'][0]['image_stamp_ns'] + moving_tracker = seeded_tracker(reference, first - STEP_NS) + fixed_tracker = seeded_tracker(reference, first - STEP_NS) + errors = [] + for event in REAL['events']: + stamp = event['image_stamp_ns'] + corners = event['pnp_observation_evidence']['corners_xy'] + rejected, reason = observe(moving_tracker, stamp, corners, None) + assert rejected is None and reason == 'pose_branch_ambiguous' + pose, reason = observe(fixed_tracker, stamp, corners, reference) + assert reason == '' and pose is not None + assert pose.quaternion_xyzw == reference.pose.quaternion_xyzw + assert pose.translation_xyz_m == reference.pose.translation_xyz_m + errors.append(pose.reprojection_error_px) + candidates, diagnostics, corrections = fixed_tracker.observation_snapshot(ROLE, stamp) + assert len(candidates) == 2 and corrections == 0 + assert diagnostics['branch_revision'] == 1 and diagnostics['branch_frozen'] + assert diagnostics['fixed_reference_verified'] is True + assert max(errors) == pytest.approx(.665772, abs=1e-5) + assert not moving_tracker._branches.states[ROLE].conflict + assert not fixed_tracker._branches.states[ROLE].conflict + + +@pytest.mark.parametrize('disturbance', ['translation', 'one_bad_corner', 'invalid_corners']) +def test_bad_fixed_images_are_never_samples_and_cannot_relocate_the_base(disturbance): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + good = np.asarray(REAL['reference_corners_xy']) + bad = good.copy() + if disturbance == 'translation': + bad += [6., 0.] + elif disturbance == 'one_bad_corner': + bad[0] += [0., 4.] + else: + bad[0] = np.nan + for index in range(1, 10): + pose, reason = observe(tracker, 1_000_000_000 + index*STEP_NS, bad, reference) + assert pose is None + assert reason == 'fixed_reference_pose_unverified' + assert not tracker.freeze_roles({ROLE: 1}) + # Image inconsistency is missing measurement evidence, not proof of a + # different physical branch. Real corner drift has its own capture gate. + assert not tracker._branches.states[ROLE].conflict + assert observe(tracker, 2_000_000_000, good, reference) == (None, 'pose_branch_unconfirmed') + assert tracker._branches.states[ROLE].pose == reference.pose + assert tracker.branch_correction_counts == {} + + +def test_short_bad_image_discards_sample_then_recovers_without_refreezing(): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + corners = np.asarray(REAL['reference_corners_xy']) + assert observe(tracker, 1_040_000_000, corners + 6, reference)[0] is None + assert not tracker.freeze_roles({ROLE: 1}) # Cannot cache the rejected base. + for index in range(1, 6): + pose, reason = observe(tracker, 1_040_000_000 + index*STEP_NS, corners, reference) + if index < 5: + assert pose is None and reason == 'pose_branch_unconfirmed' + assert not tracker.freeze_roles({ROLE: 1}) + else: + assert pose is not None and not reason + assert pose.translation_xyz_m == reference.pose.translation_xyz_m + assert tracker.freeze_roles({ROLE: 1}) + + +def test_recorded_161416_images_remain_rejected_without_latching_a_branch_conflict(): + data = json.loads((Path(__file__).parent / 'fixtures' / + 'o6_front_base_20260914_161416_quality_events.json').read_text()) + payload = data['reference_pose'] + reference = FixedTagReference(SquareTagPose(tuple(payload['rotation_xyzw']), + tuple(payload['translation_xyz_m']), 0.), 1) + first = data['events'][0]['image_stamp_ns'] + tracker = seeded_tracker(reference, first - STEP_NS) + for event in data['events']: + pose, reason = tracker.estimate(ROLE, event['pnp_observation_evidence']['corners_xy'], + tag_size_m=data['tag_size_m'], camera_matrix=data['camera_matrix'], + stamp_ns=event['image_stamp_ns'], fixed_reference=reference) + assert pose is None and reason == 'fixed_reference_pose_unverified' + error = tracker.observation_snapshot(ROLE, event['image_stamp_ns'])[1]['fixed_reference_reprojection_error_px'] + assert error > 1.5 + assert error == pytest.approx(event['pnp_observation_evidence']['candidate_diagnostics'][ + 'fixed_reference_reprojection_error_px'], abs=1e-10) + assert not tracker.freeze_roles({ROLE: 1}) + assert tracker._branches.states[ROLE].pose == reference.pose + assert not tracker._branches.states[ROLE].conflict + assert tracker.branch_correction_counts == {} + + +def test_interrupted_recovery_never_counts_failed_or_duplicate_frames(): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + corners = np.asarray(REAL['reference_corners_xy']) + for stamp, bad in ((1, True), (2, False), (3, False), (4, False), (5, True)): + assert observe(tracker, 1_000_000_000 + stamp*STEP_NS, corners + (6 if bad else 0), reference)[0] is None + for _ in range(6): + assert observe(tracker, 1_240_000_000, corners, reference)[0] is None + for index in range(7, 11): + pose, _ = observe(tracker, 1_000_000_000 + index*STEP_NS, corners, reference) + assert (pose is not None) is (index == 10) + + +def test_long_absence_reconfirms_same_frozen_pose_and_never_counts_bad_frames(): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + corners = np.asarray(REAL['reference_corners_xy']) + assert observe(tracker, 7_000_000_000, corners + 6, reference)[0] is None + for index in range(1, 6): + pose, reason = observe(tracker, 7_000_000_000 + index*STEP_NS, corners, reference) + if index < 5: + assert pose is None and reason == 'pose_branch_unconfirmed' + else: + assert pose is not None and not reason + assert tracker._branches.states[ROLE].revision == 1 + + +@pytest.mark.parametrize('revision', [True, 2]) +def test_wrong_reference_version_is_rejected(revision): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + assert observe(tracker, 1_040_000_000, REAL['reference_corners_xy'], + replace(reference, branch_revision=revision)) == (None, 'pose_branch_reference_invalid') + + +@pytest.mark.parametrize('operation', ['reset', 'newer_image']) +def test_late_fixed_projection_cannot_commit_across_reset_or_a_newer_image(monkeypatch, operation): + reference = recorded_reference() + tracker = seeded_tracker(reference, 1_000_000_000) + entered, release = Event(), Event() + project = tracking.fixed_pose_reprojection_error + calls = 0 + + def blocked(*args, **kwargs): + nonlocal calls + calls += 1 + if calls == 1: + entered.set() + assert release.wait(3) + return project(*args, **kwargs) + + monkeypatch.setattr(tracking, 'fixed_pose_reprojection_error', blocked) + with ThreadPoolExecutor(1) as pool: + pending = pool.submit(observe, tracker, 1_040_000_000, REAL['reference_corners_xy'], reference) + try: + assert entered.wait(3) + if operation == 'reset': + tracker.reset() + else: + assert observe(tracker, 1_080_000_000, REAL['reference_corners_xy'], reference)[0] + finally: + release.set() + pose, reason = pending.result(timeout=3) + assert pose is None + if operation == 'reset': + assert reason == 'pose_observation_superseded' + assert not tracker.last_candidate_diagnostics_by_role + else: + assert reason == 'pose_observation_not_new' + assert tracker.observation_snapshot(ROLE, 1_080_000_000)[1]['branch_status'] == 'tracking' + + +def test_live_fixed_evidence_keeps_corners_and_frozen_geometry_through_zero_and_sweep(): + profile, lock, capture = make_capture() + capture.profile = profile = replace(profile, measurement=replace(profile.measurement, + candidate_selection_tasks=frozenset({'thumb_pitch_dip_front'}))) + lock.begin_session() + lock.start_locking() + for stamp in range(1, 15): + capture.consume(frame(profile, stamp)) + original = capture.locked_reference('front') + zero = profile.motion.joint_zero_references['rh_thumb_cmc_pitch'] + for stamp, phase in ((20, 'joint_zero'), (21, 'sweep')): + motion = MotionCommand(phase, zero.command, 40, task_key=zero.task_key, + command_index=0, cycle=0, zero_joints=('rh_thumb_cmc_pitch',)) + rows, movement = capture.consume(frame(profile, stamp), motion) + assert movement is None + row = next(r for r in rows if r.get('joint') == 'rh_thumb_cmc_pitch') + parent, child = (row['pnp_observation_evidence'][side] for side in ('parent', 'child')) + assert parent['pose_source'] == 'verified_fixed_reference' + assert parent['corners_xy'] and parent['reprojection_valid_candidates'] + assert child['pose_source'] == 'current_image' # Moving role is never constrained. + assert parent['selected_pose']['quaternion_xyzw'] == list(original[0].quaternion_xyzw) + assert reference_branch_revisions([row], profile=profile)[('front', 'front_base')] == original[1] + candidate = next(r for r in rows if r['kind'] == 'pnp_candidate_frame') + assert candidate['roles']['front_base']['locked_reference'] is True + assert candidate['roles']['front_base']['corners_xy'] == parent['corners_xy'] + assert capture.locked_reference('front') == original diff --git a/src/linkerhand_calibration/test/test_frozen_candidate_selection.py b/src/linkerhand_calibration/test/test_frozen_candidate_selection.py new file mode 100644 index 0000000..3c80a6f --- /dev/null +++ b/src/linkerhand_calibration/test/test_frozen_candidate_selection.py @@ -0,0 +1,228 @@ +"""Articulated scoring cannot exchange an online-frozen planar solution.""" + +from copy import deepcopy +import json + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from test_o12_observation_resolution import build_data, module + + +TASK = "thumb_mcp_dip_front" +PAIRS = {"thumb_mcp": module.THUMB_ROLES[:2], "thumb_dip": module.THUMB_ROLES[1:]} + + +def frozen_capture(monkeypatch): + """Use the existing independent articulated trajectory, recording its good branch.""" + rows = build_data(monkeypatch) + frames = {row["image_stamp_ns"]: row for row in rows if "roles" in row} + for frame in frames.values(): + frame["view"] = "front" + for role in module.THUMB_ROLES: + item = frame["roles"][role] + pose = module.solve_square_tag_ippe(item["corners_xy"])[-1] + item["selected"] = {"quaternion_xyzw": list(pose.quaternion_xyzw), + "translation_xyz_m": list(pose.translation_xyz_m), + "reprojection_error_px": pose.reprojection_error_px} + for row in rows: + if "joint" not in row: + continue + row["view"] = "front" + frame = frames[row["image_stamp_ns"]] + parent, child = (deepcopy(frame["roles"][role]["selected"]) for role in PAIRS[row["joint"]]) + quaternion, translation = module._relative(parent, child) + row.update(parent_pose_common=parent, child_pose_common=child, + relative_quaternion_xyzw=quaternion.tolist(), relative_translation_xyz_m=translation.tolist()) + row["pnp_observation_evidence"] = { + field: {"tag_role": role, "tag_id": index, "pose_source": "current_image", + "selected_pose": deepcopy(frame["roles"][role]["selected"]), + "candidate_diagnostics": {"observation_stamp_ns": row["image_stamp_ns"], + "branch_status": "tracking", "branch_revision": 1, "branch_frozen": True}} + for index, (field, role) in enumerate(zip(("parent", "child"), PAIRS[row["joint"]]))} + return [{"kind": "session_start", "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION}, *rows] + + +def resolve(rows, **kwargs): + return resolve_chain_observations(rows, task_key=TASK, role_pairs=PAIRS, + solve_pose=module.solve_square_tag_ippe, **kwargs) + + +def test_same_frozen_branch_preserves_training_and_holdout(monkeypatch): + rows = frozen_capture(monkeypatch) + before = deepcopy(rows) + result, report = resolve(rows) + assert rows == before + assert report["status"] == "resolved" + assert report["training_frames"] == 72 and report["holdout_frames"] == 24 + for original, resolved in zip(rows, result): + if "joint" in original: + assert np.allclose(original["relative_quaternion_xyzw"], resolved["relative_quaternion_xyzw"], atol=1e-12) + + +@pytest.mark.parametrize("cycle", [0, 3]) +@pytest.mark.parametrize("field", ["parent", "child"]) +def test_any_frozen_role_reselection_is_rejected_before_rewriting(monkeypatch, cycle, field): + rows = frozen_capture(monkeypatch) + row = next(row for row in rows if row.get("joint") == "thumb_dip" and row["cycle"] == cycle) + role = row["pnp_observation_evidence"][field]["tag_role"] + frame = next(item for item in rows if "roles" in item and item["image_stamp_ns"] == row["image_stamp_ns"]) + pose = row["pnp_observation_evidence"][field]["selected_pose"] + pose["quaternion_xyzw"] = (Rotation.from_euler("x", 10, degrees=True) + * Rotation.from_quat(pose["quaternion_xyzw"])).as_quat().tolist() + frame["roles"][role]["selected"] = deepcopy(pose) + # The same physical role can be the adjacent joint's child or parent. + for other in rows: + if other.get("image_stamp_ns") != row["image_stamp_ns"] or "joint" not in other: + continue + for item in other["pnp_observation_evidence"].values(): + if item["tag_role"] == role: + item["selected_pose"] = deepcopy(pose) + before = deepcopy(rows) + with pytest.raises(ValueError, match=rf"frozen_branch_reselection_forbidden:.*cycle={cycle}.*role={role}"): + resolve(rows) + assert rows == before + + +@pytest.mark.parametrize("mutation", ["missing", "not_frozen", "no_revision", "wrong_stamp", "role_changed", "candidate_pose_changed"]) +def test_incomplete_frozen_evidence_never_authorizes_reselection(monkeypatch, mutation): + rows = frozen_capture(monkeypatch) + row = next(row for row in rows if row.get("joint") == "thumb_dip") + item = row["pnp_observation_evidence"]["child"] + if mutation == "missing": + row.pop("pnp_observation_evidence") + elif mutation == "not_frozen": + item["candidate_diagnostics"]["branch_frozen"] = False + elif mutation == "no_revision": + item["candidate_diagnostics"].pop("branch_revision") + elif mutation == "wrong_stamp": + item["candidate_diagnostics"]["observation_stamp_ns"] += 1 + elif mutation == "role_changed": + item["tag_role"] = "another_tag" + else: + frame = next(frame for frame in rows if "roles" in frame and frame["image_stamp_ns"] == row["image_stamp_ns"]) + frame["roles"][item["tag_role"]]["selected"]["translation_xyz_m"][0] += .01 + before = deepcopy(rows) + with pytest.raises(ValueError, match="frozen_branch_"): + resolve(rows) + assert rows == before + + +def test_numerical_reconstruction_noise_is_not_a_branch_switch(monkeypatch): + rows = frozen_capture(monkeypatch) + for row in rows: + for item in row.get("pnp_observation_evidence", {}).values(): + pose = item["selected_pose"] + pose["quaternion_xyzw"] = (Rotation.from_rotvec([1e-9, 0, 0]) + * Rotation.from_quat(pose["quaternion_xyzw"])).as_quat().tolist() + pose["translation_xyz_m"][0] += 1e-10 + assert resolve(rows)[1]["status"] == "resolved" + + +def test_cached_fixed_pose_uses_reference_revision_without_fabricated_image_diagnostics(monkeypatch): + rows = frozen_capture(monkeypatch) + base_role = module.THUMB_ROLES[0] + for row in rows: + if "roles" in row: + row["roles"][base_role].update(locked_reference=True, corners_xy=None) + for item in row.get("pnp_observation_evidence", {}).values(): + if item["tag_role"] == base_role: + item.update(pose_source="cached_fixed_reference", reference_branch_revision=1, + candidate_diagnostics={}) + assert resolve(rows, fixed_roles=(base_role,))[1]["frozen_branches_preserved"] is True + + +def verified_fixed_capture(monkeypatch): + rows = frozen_capture(monkeypatch) + role = module.THUMB_ROLES[0] + frames = {row['image_stamp_ns']: row for row in rows if 'roles' in row} + # Give the stationary role independent pinhole image evidence; the + # existing moving-role fixture still supplies its articulated candidates. + half = .016 / 2 + points = np.array([[-half, half, 0], [half, half, 0], [half, -half, 0], [-half, -half, 0]]) + for frame in frames.values(): + item = frame['roles'][role] + pose = item['selected'] + xyz = Rotation.from_quat(pose['quaternion_xyzw']).apply(points) + pose['translation_xyz_m'] + pixels = xyz @ np.asarray(frame['camera_matrix']).T + item.update(locked_reference=True, corners_xy=(pixels[:, :2] / pixels[:, 2:]).tolist()) + pose['reprojection_error_px'] = 0. + for row in rows: + for item in row.get('pnp_observation_evidence', {}).values(): + if item['tag_role'] != role: + continue + data = frames[row['image_stamp_ns']]['roles'][role] + item.update(pose_source='verified_fixed_reference', reference_branch_revision=1, + corners_xy=deepcopy(data['corners_xy']), selected_pose=deepcopy(data['selected'])) + item['candidate_diagnostics']['fixed_reference_verified'] = True + return rows, role + + +def test_verified_fixed_pose_preserves_current_corners_and_frozen_training_and_holdout(monkeypatch): + rows, role = verified_fixed_capture(monkeypatch) + before = deepcopy(rows) + result, report = resolve(rows, fixed_roles=(role,)) + assert rows == before and report['frozen_branches_preserved'] + assert report['training_frames'] == 72 and report['holdout_frames'] == 24 + for first, second in zip(rows, result): + if 'joint' in first: + np.testing.assert_allclose(first['relative_quaternion_xyzw'], second['relative_quaternion_xyzw'], atol=1e-12) + + +@pytest.mark.parametrize('cycle', [0, 3]) +@pytest.mark.parametrize('mutation', ['unverified', 'revision', 'corners_changed', 'bad_projection', 'not_fixed']) +def test_fixed_pose_marker_cannot_bypass_live_evidence_checks(monkeypatch, cycle, mutation): + rows, role = verified_fixed_capture(monkeypatch) + row = next(row for row in rows if row.get('joint') == 'thumb_mcp' and row['cycle'] == cycle) + item = row['pnp_observation_evidence']['parent'] + frame = next(row2 for row2 in rows if 'roles' in row2 and row2['image_stamp_ns'] == row['image_stamp_ns']) + if mutation == 'unverified': + item['candidate_diagnostics']['fixed_reference_verified'] = False + elif mutation == 'revision': + item['reference_branch_revision'] += 1 + elif mutation in {'corners_changed', 'bad_projection'}: + item['corners_xy'][0][0] += 10 + if mutation == 'bad_projection': + frame['roles'][role]['corners_xy'] = deepcopy(item['corners_xy']) + with pytest.raises(ValueError, match='frozen_branch_|cached fixed reference'): + resolve(rows, fixed_roles=() if mutation == 'not_fixed' else (role,)) + + +def test_previous_frozen_policy_still_preserves_recorded_branches_when_read_offline(monkeypatch): + rows = frozen_capture(monkeypatch) + rows[0]['pose_tracking_policy_version'] = 'confirmed_branch_v1' + assert resolve(rows)[1]['frozen_branches_preserved'] + + +def test_old_direct_diagnostic_selection_remains_available(monkeypatch): + rows = build_data(monkeypatch) + result, report = resolve(rows) + assert report["changed_joint_rows"] > 0 + assert result != rows + + +@pytest.mark.parametrize("policy", [None, "old_branch_policy", POSE_TRACKING_POLICY_VERSION]) +def test_formal_finalizer_cannot_remove_header_to_bypass_frozen_branch_gate(monkeypatch, tmp_path, policy): + from pathlib import Path + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.artifacts import finalization + rows = build_data(monkeypatch) + if policy is not None: + rows.insert(0, {"kind": "session_start", "pose_tracking_policy_version": policy}) + original_resolver = finalization.resolve_chain_observations + monkeypatch.setattr(finalization, "resolve_chain_observations", + lambda records, **kwargs: original_resolver(records, solve_pose=module.solve_square_tag_ippe, **kwargs)) + monkeypatch.setattr(finalization, "fit_profile_calibration", lambda *args, **kwargs: pytest.fail("fit after branch rejection")) + profile = load_bundled_hand_profile("o12_right_16") + source = Path(__file__).resolve().parents[1]/"urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" + directory = tmp_path / "session" + with pytest.raises(ValueError, match="frozen_branch_"): + finalization.finalize_profile_session(profile=profile, records=rows, + session_dir=directory, serial_number="TEST", source_urdf=source, protected_inputs={}) + report = json.loads((directory / "pose_selection_diagnostics.json").read_text()) + assert report[TASK]["publication_allowed"] is False + assert not list(directory.glob("*.urdf")) + assert not (tmp_path / "latest_passed").exists() diff --git a/src/linkerhand_calibration/test/test_frozen_tag_replay.py b/src/linkerhand_calibration/test/test_frozen_tag_replay.py index 0661248..0b7bfc8 100644 --- a/src/linkerhand_calibration/test/test_frozen_tag_replay.py +++ b/src/linkerhand_calibration/test/test_frozen_tag_replay.py @@ -126,9 +126,12 @@ def test_installation_is_unique_across_cycles_and_holdout_never_fits(tmp_path): link_by_role={"a": "proximal", "b": "distal"}, observations=[replace(training[0], cycle=3)]) slipped = [replace(row, common_from_tag=matrix_tuple(np.asarray(row.common_from_tag) @ transform((0.12, 0, 0)))) if row.cycle == 2 else row for row in training] - with pytest.raises(ValueError, match="not rigid"): + from linkerhand_calibration.core.fitting.tag_installation import TagInstallationFailure + with pytest.raises(TagInstallationFailure, match="tag_installation_not_rigid") as failure: fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, link_by_role={"a": "proximal", "b": "distal"}, observations=slipped) + assert set(failure.value.diagnostics) == {"a", "b"} + assert all(not row["passed"] for row in failure.value.diagnostics.values()) # Same physical Tag observed through two tasks is deduplicated, not fitted twice. duplicate = fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, link_by_role={"a": "proximal", "b": "distal"}, observations=training+training) diff --git a/src/linkerhand_calibration/test/test_held_joint_replay.py b/src/linkerhand_calibration/test/test_held_joint_replay.py new file mode 100644 index 0000000..e82b1bd --- /dev/null +++ b/src/linkerhand_calibration/test/test_held_joint_replay.py @@ -0,0 +1,86 @@ +"""A held ancestor's hysteresis is part of the physical FK condition.""" + +from types import SimpleNamespace as NS + +import numpy as np +import pytest + +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.fitting.tag_installation import TagInstallationFailure, matrix_tuple +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.tag_acceptance import validate_serialized_tag_holdout +from linkerhand_calibration.runtime.artifacts.evidence import prepare_tag_replay +from test_frozen_tag_replay import transform + + +def virtual_capture(tmp_path): + source = tmp_path / "source.urdf" + source.write_text(''' + + + + + + ''') + tasks = tuple(NS(key=name, joints=(name,), command_index=i, start_value=0., end_value=1.) + for i, name in enumerate(("root", "tip"))) + profile = NS(command=NS(unit="rad", urdf_joint_by_joint={}), + measurement=NS(measurements={name:NS(view="front", child_role=name, kind="relative_rotation") + for name in ("root", "tip")}), motion=NS(tasks=tasks), + vision=NS(views=(NS(name="front", tags=( + NS(role="root", link="proximal"), NS(role="tip", link="distal"))),))) + mappings = {name: JointMapping(name,i,"feedback_rad",(0.,1.),(0.,1.),(delta,1+delta),(-delta,1-delta)) + for i,(name,delta) in enumerate((("root",.08),("tip",.025)))} + base=transform((.1,-.2,.3),(.1,.2,.7)) + mounts={"root":transform((.2,.3,-.1),(.01,.02,.03)), + "tip":transform((-.3,.1,.2),(.015,.01,.025))} + rows=[] + for cycle in range(4): + for task in tasks: + for direction in ("increasing","decreasing"): + held="increasing" if (cycle+(direction=="increasing"))%2 else "decreasing" + for i,x in enumerate(np.linspace(.05,.95,50)): + sdk=[.45,.55];sdk[task.command_index]=float(x) + directions=[held,held];directions[task.command_index]=direction + # Elementary ground truth, independent of URDF FK and fit. + root=sdk[0]+(.08 if directions[0]=="increasing" else -.08) + tip=sdk[1]+(.025 if directions[1]=="increasing" else -.025) + pose=transform((0,0,root)) + if task.key=="tip":pose=pose@transform(xyz=(.04,0,.05))@transform((0,tip,0)) + rows.append({"joint":task.key,"task_name":task.key,"view":"front","cycle":cycle, + "sample_id":f"front:{task.key}:{cycle}:{direction}:{i}","direction":direction, + "state_rad":sdk,"command_vector_rad":sdk,"command_direction_by_index":directions, + "child_pose_common":matrix_tuple(base@pose@mounts[task.key])}) + return profile,source,mappings,base,rows + + +def test_held_direction_is_preserved_through_training_and_final_urdf_replay(tmp_path): + profile,source,mappings,base,rows=virtual_capture(tmp_path) + args=dict(profile=profile,source_model=UrdfKinematicModel(source), + plan=NS(mimic_output={},zero_offsets_rad={}),output_mappings=mappings,common_from_base=base) + # Dropping known arrival branches falsely makes a perfectly rigid Tag + # appear to move by >4 degrees between captures of its child joint. + without_history=[{k:v for k,v in row.items() if k!="command_direction_by_index"} for row in rows] + with pytest.raises(TagInstallationFailure): + prepare_tag_replay(**args,records=without_history) + replay=prepare_tag_replay(**args,records=rows) + assert max(m.training_rotation_p95_deg for m in replay.installations.values())<1e-6 + payload={"schema_version":7,"profile_id":"synthetic","curve_input_domain":"feedback_rad", + "joints":{name:{"urdf_joint":name,"motor_index":m.motor_index, + "curve_input_knots_rad":list(m.knots),"angle_rad":list(m.angle_rad), + "increasing_rad":list(m.increasing_rad),"decreasing_rad":list(m.decreasing_rad)} + for name,m in mappings.items()}} + metrics=validate_serialized_tag_holdout(corrected_urdf=source,payload=payload, + common_from_base=replay.common_from_base,installations=replay.installations, + observations=replay.holdout,required_roles=replay.required_roles) + assert max(r["maximum_deg"] for r in metrics.values())<1e-6 + + +@pytest.mark.parametrize("history", [["increasing"], ["increasing","invalid"], "increasing"]) +def test_partial_or_invalid_direction_history_cannot_silently_use_mean(tmp_path, history): + profile,source,mappings,base,rows=virtual_capture(tmp_path) + rows[0]["command_direction_by_index"]=history + with pytest.raises(ValueError,match="invalid SDK direction history"): + prepare_tag_replay(profile=profile,source_model=UrdfKinematicModel(source), + plan=NS(mimic_output={},zero_offsets_rad={}),output_mappings=mappings, + common_from_base=base,records=rows) diff --git a/src/linkerhand_calibration/test/test_image_error_budget.py b/src/linkerhand_calibration/test/test_image_error_budget.py new file mode 100644 index 0000000..1445a6e --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_error_budget.py @@ -0,0 +1,124 @@ +"""Independent truth separates measurement noise from motion approximation.""" + +from dataclasses import replace +import math + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.tag_installation import FrozenTagInstallation +from linkerhand_calibration.core.urdf.image_acceptance import ( + ImageHoldoutFailure, TagImageObservation, validate_serialized_image_holdout, +) +from linkerhand_calibration.core.urdf.image_consistency import _rigid_projection +from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout + + +def known_scene(tmp_path, *, focal, error_deg=.5, translation_m=0.): + source = tmp_path / "known.urdf" + source.write_text(''' + + + ''') + payload = dict(schema_version=7, profile_id="KNOWN/right/one/v1", curve_input_domain="feedback_rad", + joints={"drive": dict(urdf_joint="drive", motor_index=0, + curve_input_knots_rad=[0., 1.], angle_rad=[0., 1.])}) + base, mount = np.eye(4), np.eye(4) + base[2, 3], mount[0, 3] = .8, .1 + frozen = FrozenTagInstallation("tip", "finger", tuple(map(tuple, mount)), ("train:0",), 0., 0.) + objects = np.array([[-.008, .008, 0], [.008, .008, 0], [.008, -.008, 0], [-.008, -.008, 0]]) + camera = np.array([[focal, 0, 812.], [0, focal, 620.], [0, 0, 1.]]) + poses, images = [], [] + for index, sdk in enumerate(np.linspace(.1, .9, 40)): + angle = float(sdk) + math.radians(error_deg) + rotation = np.array([[math.cos(angle), -math.sin(angle), 0], + [math.sin(angle), math.cos(angle), 0], [0, 0, 1.]]) + pose = np.eye(4) + pose[:3, :3] = rotation + pose[:3, 3] = [.1*math.cos(angle)+translation_m, .1*math.sin(angle), .8] + row = TagHoldout(f"camera:{index}", "tip", 3, (float(sdk),), {}, tuple(map(tuple, pose))) + points = (objects @ rotation.T + pose[:3, 3]) @ camera.T + corners = points[:, :2] / points[:, 2:] + poses.append(row) + images.append(TagImageObservation(row.sample_id, row.role, 3, row.sdk_values, (), + "camera", .016, tuple(map(tuple, corners)), tuple(map(tuple, camera)), + tuple(map(tuple, np.eye(4))), task_name="drive")) + return dict(corrected_urdf=source, payload=payload, common_from_base=base, + installations={"tip": frozen}, required_roles=("tip",), observations=images, + independent_observations=poses, input_kind="feedback", holdout_cycle=3, minimum_samples=40) + + +@pytest.mark.parametrize("focal", [1000., 3500.]) +def test_same_allowed_physical_error_does_not_depend_on_focal_length(tmp_path, focal): + scene = known_scene(tmp_path, focal=focal) + physical = {key: value for key, value in scene.items() if key not in {"observations", "independent_observations"}} + physical["observations"] = scene["independent_observations"] + validate_serialized_tag_holdout(**physical) + metrics = validate_serialized_image_holdout(**scene)["tip"] + assert metrics["independent_motion"]["mae_deg"] == pytest.approx(.5) + assert metrics["maximum_px"] < 1e-8 + assert (metrics["nominal_projection"]["maximum_px"] > 1.5) == (focal == 3500.) + + +@pytest.mark.parametrize("error", [dict(error_deg=4.), dict(error_deg=0., translation_m=.004)]) +def test_perfect_measurement_pixels_cannot_bypass_physical_accuracy(tmp_path, error): + scene = known_scene(tmp_path, focal=3500., **error) + with pytest.raises(ImageHoldoutFailure) as caught: + validate_serialized_image_holdout(**scene) + report = caught.value.report["tip"] + assert report["maximum_px"] < 1e-8 + assert not report["independent_motion"]["passed"] + + +def test_extra_view_witness_is_separate_from_independent_accuracy(tmp_path): + scene = known_scene(tmp_path, focal=3500.) + extra = replace(scene["observations"][0], sample_id="camera:extra", task_name="another_motion") + scene["observations"].append(extra) + report = validate_serialized_image_holdout(**scene)["tip"] + assert report["count"] == 41 + assert report["independent_motion"]["count"] == 40 + witness = report["auxiliary_consistency_by_task"]["another_motion"] + assert witness["passed"] and witness["maximum_px"] <= 1.5 + assert witness["interpretation"] == "feasible_pose_displacement_only" + + +def test_stationary_images_cannot_dilute_inconsistent_motion(tmp_path): + scene = known_scene(tmp_path, focal=3500., error_deg=0.) + original = scene["observations"][0] + scene["observations"].extend(replace(original, sample_id=f"camera:static:{i}", + task_name="stationary") for i in range(100)) + scene["observations"].append(replace(original, sample_id="camera:bad", task_name="moving", + corners_xy=tuple((x+80, y) for x, y in original.corners_xy))) + with pytest.raises(ImageHoldoutFailure) as caught: + validate_serialized_image_holdout(**scene) + report = caught.value.report["tip"] + assert report["independent_motion"]["passed"] + assert report["auxiliary_consistency_by_task"]["stationary"]["passed"] + assert not report["auxiliary_consistency_by_task"]["moving"]["passed"] + + +@pytest.mark.parametrize("mutation", ["missing_pose", "sdk", "direction", "duplicate_pose"]) +def test_independent_pose_identity_and_inputs_cannot_be_replaced_by_witnesses(tmp_path, mutation): + scene = known_scene(tmp_path, focal=3500.) + rows = scene["independent_observations"] + if mutation == "missing_pose": rows.pop() + elif mutation == "sdk": rows[0] = replace(rows[0], sdk_values=(.2,)) + elif mutation == "direction": rows[0] = replace(rows[0], sdk_directions_by_joint={"drive": "increasing"}) + else: rows.append(rows[0]) + with pytest.raises(ValueError, match="final_image_holdout"): + validate_serialized_image_holdout(**scene) + + +@pytest.mark.parametrize("x", [np.zeros(6), np.array([.6, -.3, .2, -.8, .2, .5])]) +def test_rigid_projection_jacobian_matches_independent_finite_differences(x): + pose = np.eye(4) + pose[:3, :3] = Rotation.from_euler("xyz", [.3, -.6, .2]).as_matrix() + pose[:3, 3] = [.08, -.03, .5] + camera = np.array([[3500., 4., 820.], [0, 3600., 650.], [0, 0, 1.]]) + objects = np.array([[-.008, .008, 0], [.008, .008, 0], [.008, -.008, 0], [-.008, -.008, 0]]) + args = (pose, camera, objects, np.zeros((4, 2)), np.deg2rad(3.), .003) + _, analytic = _rigid_projection(x, *args) + numeric = np.stack([(_rigid_projection(x+step, *args)[0]-_rigid_projection(x-step, *args)[0]) / 2e-5 + for step in np.eye(6)*1e-5], axis=-1) + np.testing.assert_allclose(analytic, numeric, rtol=2e-6, atol=2e-8) diff --git a/src/linkerhand_calibration/test/test_image_model_selection.py b/src/linkerhand_calibration/test/test_image_model_selection.py new file mode 100644 index 0000000..1dfb526 --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_model_selection.py @@ -0,0 +1,126 @@ +"""Selection stays independent of holdout quality, order and duplicate starts.""" +from dataclasses import replace +import gzip +from itertools import permutations +import json +from pathlib import Path + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.tag_pose.image_model_selection import ( + compare_image_losses, equivalent_frame_selections, holm_adjusted_probabilities, + training_candidate_index, +) +from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionHypothesis +from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageRoleQuality + + +def hypothesis(training, validation, reason=""): + return ImageMotionHypothesis((("root", 0), ("moving", 0)), True, reason, + (ImageRoleQuality("moving", 30, training, training, training),), + (ImageRoleQuality("moving", 30, validation, validation, validation),), training_accepted=True) + + +def test_holdout_cannot_select_or_replace_training_candidate(): + candidates = (hypothesis(.2, 1.6, "image_motion_reprojection_failed"), hypothesis(.3, .1)) + assert training_candidate_index(candidates) == 0 + changed = tuple(replace(h, validation_quality=(), reason="holdout_unavailable") for h in candidates) + assert training_candidate_index(changed) == 0 + assert training_candidate_index(tuple(replace(h, training_accepted=False) for h in candidates)) is None + + +def test_failed_selected_holdout_vetoes_resolver_without_trying_runner_up(monkeypatch): + from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver + from test_motion_image_diagnostics import frames, RELATIONS + + outcomes = iter(((hypothesis(.2, 1.6, "image_motion_reprojection_failed"), object(), np.array([1.6])), + (hypothesis(.3, .1), object(), np.array([.1])))) + monkeypatch.setattr(solver, "_fit_hypothesis", lambda *args, **kwargs: next(outcomes)) + result = solver.resolve_image_motion(frames(alternatives=True), RELATIONS) + assert not result.resolved and result.model is None + assert result.reason == "image_motion_no_observable_image_model" + + +def test_validation_exception_preserves_training_identity(monkeypatch): + from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver + from linkerhand_calibration.core.geometry.tag_pose.motion_image_solver import ImageHingeBundle + from test_motion_image_diagnostics import frames, RELATIONS + + def fail(*args, **kwargs): + raise ValueError("validation_failed") + monkeypatch.setattr(ImageHingeBundle, "validate", fail) + result = solver.resolve_image_motion(frames(), RELATIONS) + assert not result.resolved + assert result.hypotheses[0].training_accepted + assert result.hypotheses[0].reason == "validation_failed" + + +@pytest.mark.parametrize("values,expected", [ + ([(0, .008), (1, 0.), (2, 0.)], {0: .008, 1: 0., 2: 0.}), + ([(0, .008), (1, .009), (2, 0.)], {0: .016, 1: .016, 2: 0.}), + ([(0, .001), (1, .005), (2, .008)], {0: .003, 1: .01, 2: .01}), +]) +def test_holm_preserves_family_level_and_is_order_independent(values, expected): + for ordering in permutations(values): + assert dict(holm_adjusted_probabilities(ordering)) == pytest.approx(expected) + + +def test_holm_rejects_invalid_probabilities_and_duplicate_ids(): + assert dict(holm_adjusted_probabilities([(0, float("nan")), (1, -.1), (2, 1.1)])) == {0: 1., 1: 1., 2: 1.} + assert holm_adjusted_probabilities([]) == () + with pytest.raises(ValueError, match="duplicate"): + holm_adjusted_probabilities([(0, .01), (0, .02)]) + + +def test_comparison_keeps_pixel_margin_and_rejects_isolated_outlier(): + angles = np.linspace(0., 1.2, 72)[:, None] + baseline = np.full(72, .3) + assert compare_image_losses(baseline, baseline + .02, angles, .03) == 1. + assert compare_image_losses(baseline, baseline + .2, angles, .03) < .001 + outlier = baseline.copy() + outlier[-2:] += 20 + assert compare_image_losses(baseline, outlier, angles, .03) > .01 + assert compare_image_losses(baseline, baseline + .2, np.zeros_like(angles), .03) == 1. + assert compare_image_losses(baseline, baseline + np.nan, angles, .03) == 1. + # Exact image repetitions must not count as new motion evidence. + repeated = compare_image_losses(np.repeat(baseline, 4), np.repeat(baseline + .2, 4), + np.repeat(angles, 4, axis=0), .03) + assert repeated == compare_image_losses(baseline, baseline + .2, angles, .03) + + +def test_equivalence_matches_roles_and_rejects_missing_or_duplicate_roles(): + from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionFrameSelection + from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRoleSelection + from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + + pose = SquareTagPose((0., 0., 0., 1.), (0., 0., 1.), .1) + first = ImageMotionFrameSelection(True, "", (MotionRoleSelection("root", pose), MotionRoleSelection("tip", pose))) + assert equivalent_frame_selections(first, replace(first, selections=first.selections[::-1])) + assert not equivalent_frame_selections(first, replace(first, selections=first.selections[:1])) + assert not equivalent_frame_selections(first, replace(first, selections=(first.selections[0],)*2)) + assert not equivalent_frame_selections(first, replace(first, resolved=False)) + + +@pytest.mark.parametrize("attempt,frames_count", [(0, 96), (1, 95)]) +def test_recorded_side_attempts_pass_with_original_geometry_and_quality_limits(attempt, frames_count): + from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry + from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion + from test_production_image_motion import recorded_image_frames + + fixture = Path(__file__).parent / "fixtures/o6_side_preparation_193254.json.gz" + case = json.loads(gzip.decompress(fixture.read_bytes()))["cases"][attempt] + frames = recorded_image_frames(case["frames"]) + assert len(frames) == frames_count + result = resolve_image_motion(frames, tuple(MotionRelation(**r) for r in case["relations"]), + geometry_constraints=tuple(ParallelAxisGeometry(**c) for c in case["constraints"]), + source_urdf_sha256=case["source_urdf_sha256"]) + assert result.resolved, result.reason + assert result.model.maximum_reprojection_error_px == 1.5 + assert result.model.reprojection_tie_px == .03 + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + winner = training_candidate_index(result.hypotheses) + assert result.model.branches == result.hypotheses[winner].branches + adjusted = result.hypotheses[winner].comparison_adjusted_p_values + assert len(adjusted) == 3 and max(p for _, p in adjusted) <= .01 diff --git a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py new file mode 100644 index 0000000..24127a2 --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py @@ -0,0 +1,195 @@ +"""Capture persists the actual frozen image solver boundary for read-back.""" + +from copy import deepcopy +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.image_motion_replay import replay_image_models, validate_image_sample +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import image_motion_model_from_dict +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose +from linkerhand_calibration.runtime import capture as capture_module +from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from test_image_motion_provenance import image_motion_capture + + +def capture_rig(monkeypatch): + profile, records = image_motion_capture() + profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset())) + original = next(row for row in records if row["kind"] == "pnp_candidate_frame") + task = profile.motion.tasks[0] + moving = next(role for role, item in original["roles"].items() if "frozen_image_model" in item) + root = next(role for role in original["roles"] if role != moving) + model = image_motion_model_from_dict(original["roles"][moving]["frozen_image_model"]) + branch = SimpleNamespace(task_name=task.key, view=task.view, constraints=(), + image_model=model, evidence_ids=((moving, "a" * 64),)) + + class Tracker: + maximum_reprojection_error_px = 1.5 + + def estimate(self, role, corners, *, stamp_ns, **kwargs): + self.stamp = stamp_ns + return SquareTagPose(**original["roles"][role]["reprojection_valid_candidates"][0]), "" + + def observation_snapshot(self, role, stamp_ns): + item = original["roles"][role] + diagnostics = dict(item["candidate_diagnostics"], observation_stamp_ns=stamp_ns) + return tuple(SquareTagPose(**p) for p in item["reprojection_valid_candidates"]), diagnostics, 0 + + def freeze_roles(self, *args, **kwargs): + return True + + tracker = Tracker() + lock = SimpleNamespace(fixed_tag_by_view={task.view: 6}, missing_views=set(), locking_enabled=False, + observe=lambda *args: None) + capture = ObservationCapture(profile, reference_lock=lock, trackers={task.view: tracker}, + extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) + capture.locked_poses[task.view] = SquareTagPose(**original["roles"][root]["selected"]) + capture.locked_revisions[task.view] = 7 + capture.install_motion_model(branch) + + class SelectedEvidence: + def observation_snapshot(self, role, stamp_ns): + candidates, diagnostics, corrections = tracker.observation_snapshot(role, stamp_ns) + if role == moving: + diagnostics.update(frozen_image_model=original["roles"][role]["frozen_image_model"], + image_model_sha256=original["roles"][role]["image_model_sha256"], + motion_evidence_verified=True, motion_evidence_id="a" * 64) + return candidates, diagnostics, corrections + + def select(frame, model, tracker, selected, *, base, cached): + return {moving: SquareTagPose(**original["roles"][moving]["selected"])}, {}, SelectedEvidence() + + monkeypatch.setattr(capture_module, "select_motion_poses", select) + values = tuple(profile.command.baseline_values) + frame = CaptureFrame(task.view, 1000, np.asarray(original["camera_matrix"]), + {role: np.asarray(item["corners_xy"]) for role, item in original["roles"].items()}, values, values) + return profile, capture, frame, branch + + +@pytest.mark.parametrize("phase", ["joint_zero", "steady", "sweep"]) +def test_actual_capture_writes_replayable_image_model_when_candidate_strategy_is_empty(monkeypatch, phase): + profile, capture, frame, branch = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + motion = MotionCommand(phase, profile.command.baseline_values, 40., task_key=task.key, + command_index=task.command_index, cycle=0, direction="increasing", zero_joints=task.joints) + rows, movement = capture.consume(frame, motion) + assert movement is None + candidates = next(row for row in rows if row["kind"] == "pnp_candidate_frame") + sample = next(row for row in rows if row["kind"] in {"joint_sample", "joint_zero_sample"}) + assert candidates["sample_phase"] == phase + replayed = replay_image_models(candidates) + validate_image_sample(sample, candidates, replayed) + moving = branch.image_model.geometry[0].relation.child_role + evidence = candidates["roles"][moving] + assert evidence["pose_source"] == "image_constrained_current_image" + assert evidence["selected"] != evidence["reprojection_valid_candidates"][0] + + +def test_unconfirmed_diagnostic_does_not_emit_formal_image_candidate_frame(monkeypatch): + profile, capture, frame, _ = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + motion = MotionCommand("sweep", profile.command.baseline_values, 40., task_key=task.key, + command_index=task.command_index, cycle=0, direction="increasing") + rows, _ = capture.consume(frame, motion, diagnostic=True, accepted_joints=frozenset()) + assert [row["kind"] for row in rows] == ["diagnostic_observation_frame"] + + +@pytest.mark.parametrize("feedback_available", [True, False]) +def test_diagnostic_hold_keeps_transient_raw_images_before_formal_steady_window(monkeypatch, feedback_available): + profile, capture, frame, _ = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + command, feedback, target = [list(profile.command.baseline_values) for _ in range(3)] + command[task.command_index], feedback[task.command_index], target[task.command_index] = 224., 230., 192. + frame = replace(frame, command=tuple(command), feedback=tuple(feedback) if feedback_available else None) + motion = MotionCommand("steady", tuple(target), 40., task_key=task.key, + command_index=task.command_index, cycle=0, direction="decreasing", steady_index=0, + minimum_hold_seconds=10.) + rows, _ = capture.consume(frame, motion, diagnostic=True, allow_recording=False, + accepted_joints=frozenset(task.joints)) + assert [row["kind"] for row in rows] == ["diagnostic_observation_frame"] + raw = rows[0] + assert raw["sample_phase"] == "steady" and raw["steady_target"] == 192. + assert raw["steady_index"] == 0 and raw["minimum_hold_seconds"] == 10. + assert raw["target_command_vector"] == target and raw["command_vector"] == command + assert raw["feedback_vector"] == (feedback if feedback_available else None) + assert not raw["accepted_sample_window_open"] and not raw["is_accepted_joint_sample"] + assert raw["tags"] and raw["camera_matrix"] == frame.camera_matrix.tolist() + + +def test_formal_capture_does_not_record_unstable_steady_images(monkeypatch): + profile, capture, frame, _ = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + motion = MotionCommand("steady", profile.command.baseline_values, 40., task_key=task.key, + command_index=task.command_index, cycle=0, direction="decreasing", steady_index=0) + rows, _ = capture.consume(frame, motion, diagnostic=False, allow_recording=False) + assert not rows + + +@pytest.mark.parametrize("phase", ["joint_zero", "sweep"]) +def test_missing_image_model_root_cannot_be_replaced_by_stationary_cache(monkeypatch, phase): + profile, capture, frame, branch = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + root = branch.image_model.geometry[0].relation.parent_role + frame = replace(frame, corners={role: points for role, points in frame.corners.items() if role != root}) + motion = MotionCommand(phase, profile.command.baseline_values, 40., task_key=task.key, + command_index=task.command_index, cycle=0, direction="increasing", zero_joints=task.joints) + rows, movement = capture.consume(frame, motion) + assert movement is None + assert not any(row["kind"] in {"joint_sample", "joint_zero_sample", "pnp_candidate_frame"} for row in rows) + + +def test_installed_image_model_is_managed_even_without_current_task(monkeypatch): + _, capture, frame, branch = capture_rig(monkeypatch) + tracker = capture.trackers[frame.view] + estimate = tracker.estimate + children = {item.relation.child_role for item in branch.image_model.geometry} + observed = set() + + def check_managed(role, *args, **kwargs): + if role in children: + assert kwargs.get("motion_managed") is True + observed.add(role) + return estimate(role, *args, **kwargs) + + monkeypatch.setattr(tracker, "estimate", check_managed) + rows, movement = capture.consume(frame, None) + assert observed == children and movement is None + assert not rows + + +def test_current_task_resolves_installed_parent_model_first_and_keeps_ancestral_roles(monkeypatch): + profile, capture, _, branch = capture_rig(monkeypatch) + parent = deepcopy(branch) + parent.task_name = "previous_joint_task" + first = parent.image_model.geometry[0] + child = deepcopy(branch) + child.image_model = replace(branch.image_model, + geometry=(replace(first, relation=replace(first.relation, + parent_role=first.relation.child_role, child_role="terminal")),), + tag_sizes=((first.relation.child_role, .016), ("terminal", .016)), + branches=((first.relation.child_role, 0), ("terminal", 0))) + child.evidence_ids = (("terminal", "b" * 64),) + capture._motion_models.clear() + capture.install_motion_model(parent) + capture.install_motion_model(child) + motion = SimpleNamespace(task_key=child.task_name) + assert capture._motion_model_schedule(child.view, motion) == (parent, child) + evidence = {"terminal": {"frozen_image_model": {"tag_sizes": child.image_model.tag_sizes}}, + first.relation.child_role: {"frozen_image_model": {"tag_sizes": parent.image_model.tag_sizes}}} + assert capture._image_model_roles({"terminal"}, evidence) == { + "terminal", first.relation.parent_role, first.relation.child_role} + + +def test_installation_cannot_change_an_existing_physical_tag_model(monkeypatch): + _, capture, _, branch = capture_rig(monkeypatch) + replacement = deepcopy(branch) + replacement.task_name = "another_task" + geometry = replacement.image_model.geometry[0] + replacement.image_model = replace(replacement.image_model, + geometry=(replace(geometry, pivot_parent_xyz_m=(.1, .1, .1)),)) + with pytest.raises(ValueError, match="image_motion_model_replacement_forbidden"): + capture.install_motion_model(replacement) diff --git a/src/linkerhand_calibration/test/test_image_motion_live_flow.py b/src/linkerhand_calibration/test/test_image_motion_live_flow.py new file mode 100644 index 0000000..56ede84 --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_motion_live_flow.py @@ -0,0 +1,75 @@ +"""One coordinator transport run using the complete recorded thumb image arc. + +SDK commands select the nearest recorded image; timestamps are virtual. This +exercises the production image solver, new tracker commit and evidence writer, +not hand accuracy or an independent camera truth dataset. +""" +import gzip +import json +from pathlib import Path + +import numpy as np +import pytest + +from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection +from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance, validate_source_geometry +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from test_diagnostic_coordinator import run_until, THUMB_JOINTS +from runtime_host_fixture import coordinator_fixture, ready + + +def test_recorded_image_geometry_completes_thumb_coordinator_without_publication(tmp_path, monkeypatch): + import test_diagnostic_coordinator as transport + payload = json.loads(gzip.decompress((Path(__file__).parent / "fixtures" / + "o6_image_live_flow_200044.json.gz").read_bytes())) + assert payload["source_sha256"] == "52f9ed7510fc58a4271532a2dc13a678f2e4aa2f1dc3153bf749fcf377cb23c5" + host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb") + ready(host, clock) + assert host.branch_initialization.image_geometry + for view, data in payload["views"].items(): + host.cameras.matrices[view] = np.asarray(data["camera_matrix"]) + host.cameras.image_sizes[view] = (2048, 2048) + monkeypatch.setattr(host.finalization, "start", lambda *a, **kw: pytest.fail("diagnostic started finalizer")) + assert host.start().success + + def recorded_frame(host, view, feedback): + phase = host._motion.phase if host._motion is not None else "joint_zero" + target = feedback[0 if view == "front" else 1] + pool = payload["views"][view]["frames"] + # Preparation exclusively uses recorded preparation images. A later + # direction cannot silently enter the geometry-training callback. + selected_phase = "sweep" if phase == "sweep" else "zero_approach" + available = [row for row in pool if row["phase"] == selected_phase] + if phase == "sweep": + direction = host._motion.direction + matching = [row for row in available if row["direction"] == direction] + available = matching or available + row = min(available, key=lambda r: abs(r["input"] - target)) + return DetectionInput(view, host.ports.clock_ns(), tuple(TagDetection(tag["id"], + tuple(tuple(p) for p in tag["corners"]), 0, 100.) for tag in row["tags"])) + + monkeypatch.setattr(transport, "projected_frame", recorded_frame) + try: + run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE) + assert set(host.execution.zero_references) == THUMB_JOINTS + assert not host.execution.diagnostic_zero_attempts + assert not host.finalization.started and not host.final_json and not host.final_urdf + records = host.raw_records + initializations = [r for r in records if r["kind"] == "motion_branch_initialization"] + assert len(initializations) == 2 and all(r["status"] == "resolved" for r in initializations) + assert all(r.get("frozen_image_model") and r.get("image_model_sha256") for r in initializations) + samples = [r for r in records if r["kind"] == "joint_sample"] + assert {r["joint"] for r in samples} == THUMB_JOINTS + assert {r["direction"] for r in samples} == {"increasing", "decreasing"} + assert len(host.execution.completed_units) == 4 + proof = validate_motion_provenance(host.profile, records, required=True) + assert proof["motion_evidence_verified"] and not proof["is_accuracy_certificate"] + validate_source_geometry(host.profile, host.parameters.source_urdf, records) + from linkerhand_calibration.core.artifacts.storage import load_jsonl + journal = load_jsonl(host.raw_path) + assert json.loads(json.dumps(records[0])) == journal[0] + assert validate_motion_provenance(host.profile, journal, required=True) == proof + with pytest.raises(ValueError, match="image_model_source_urdf_changed"): + validate_motion_provenance(host.profile, records[1:], required=True) + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_image_motion_provenance.py b/src/linkerhand_calibration/test/test_image_motion_provenance.py new file mode 100644 index 0000000..5fd42df --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_motion_provenance.py @@ -0,0 +1,265 @@ +"""Constrained image poses keep raw candidates and survive immutable replay.""" + +from copy import deepcopy +from dataclasses import asdict, replace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.image_motion_replay import ( + IMAGE_POSE_SOURCE, image_model_sha256, replay_image_models, validate_image_sample, +) +from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionModel +from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import select_image_motion_frame +from test_motion_image_diagnostics import MATRIX, frames + + +def recorded_image(*, stamp=None, cycle=0): + model = ImageMotionModel((ImageHingeGeometry(MotionRelation("hinge", "root", "moving"), + (0., 0., 1.), (0., 0., 0., 1.), (.01, .02, 0.), (.035, 0., 0.)),), MATRIX, + (("root", .016), ("moving", .016)), (1, 2), (1,), (2,), (("root", 0), ("moving", 0))) + original = frames(depth_error=.008)[24] + if stamp is not None: + original = replace(original, evidence=replace(original.evidence, stamp_ns=stamp)) + result = select_image_motion_frame(model, original) + assert result.resolved, result + selected = {item.role: asdict(item.pose) for item in result.selections} + observations = {item.role: item for item in original.observations} + stamp = original.evidence.stamp_ns + frame = {"kind": "pnp_candidate_frame", "task_name": "task", "view": "front", + "image_stamp_ns": stamp, "cycle": cycle, "direction": "increasing", "attempt": 1, + "tag_size_m": .016, "camera_matrix": MATRIX, + "camera_matrix_source": "CameraInfo.P[:3,:3]", "input_is_rectified": True, + "roles": {}} + evidence = {} + for candidates in original.evidence.roles: + role = candidates.role + data = {"corners_xy": observations[role].corners_xy, "tag_size_m": .016, + "selected": selected[role], "reprojection_valid_candidates": [asdict(p) for p in candidates.candidates], + "pose_source": IMAGE_POSE_SOURCE if role == "moving" else "verified_fixed_reference", + "locked_reference": role == "root"} + if role == "moving": + data.update(frozen_image_model=asdict(model), image_model_sha256=image_model_sha256(asdict(model))) + frame["roles"][role] = data + evidence["parent" if role == "root" else "child"] = {**data, "tag_role": role, + "selected_pose": selected[role], "reference_branch_revision": 1, + "candidate_diagnostics": {"branch_frozen": True, "branch_revision": 1, + "branch_status": "tracking", "observation_stamp_ns": stamp, + "fixed_reference_verified": role == "root"}} + data["candidate_diagnostics"] = evidence["parent" if role == "root" else "child"]["candidate_diagnostics"] + rp, rc = (Rotation.from_quat(selected[r]["quaternion_xyzw"]) for r in ("root", "moving")) + row = {key: frame[key] for key in ("task_name", "view", "image_stamp_ns", "cycle", "direction", "attempt")} + row.update(kind="joint_sample", joint="hinge", sample_phase="sweep", pnp_observation_evidence=evidence, + parent_pose_common=selected["root"], child_pose_common=selected["moving"], + relative_quaternion_xyzw=(rp.inv()*rc).as_quat().tolist(), + relative_translation_xyz_m=rp.inv().apply(np.subtract(selected["moving"]["translation_xyz_m"], + selected["root"]["translation_xyz_m"])).tolist()) + return frame, row + + +def test_image_pose_replay_is_distinct_from_raw_pnp_and_never_mutates_capture(): + frame, row = recorded_image() + before = deepcopy((frame, row)) + replay = replay_image_models(frame) + validate_image_sample(row, frame, replay) + assert frame == before[0] and row == before[1] + assert frame["roles"]["moving"]["selected"] != frame["roles"]["moving"]["reprojection_valid_candidates"][0] + + +@pytest.mark.parametrize("mutation", ["model", "hash", "projection", "size", "corners", "selected", + "source", "raw_candidates", "future_support"]) +def test_frozen_model_and_current_image_evidence_cannot_be_substituted(mutation): + frame, _ = recorded_image() + moving = frame["roles"]["moving"] + if mutation == "model": + moving["frozen_image_model"]["geometry"][0]["pivot_parent_xyz_m"] = (.2, .2, .2) + elif mutation == "hash": + moving["image_model_sha256"] = "a" * 64 + elif mutation == "projection": + frame["camera_matrix_source"] = "CameraInfo.K" + elif mutation == "size": + moving["tag_size_m"] = .02 + elif mutation == "corners": + moving["corners_xy"] = tuple((x + 15., y) for x, y in moving["corners_xy"]) + elif mutation == "selected": + moving["selected"] = {**moving["selected"], "translation_xyz_m": (.1, .1, .8)} + elif mutation == "source": + moving["pose_source"] = "current_image" + elif mutation == "raw_candidates": + moving.pop("reprojection_valid_candidates") + else: + frame["image_stamp_ns"] = 2 + with pytest.raises(ValueError, match="image_motion_provenance_"): + replay_image_models(frame) + + +@pytest.mark.parametrize("mutation", ["sample_model", "sample_corners", "sample_pose", "relative", "identity", "marker"]) +def test_sample_must_match_the_replayed_frozen_image(mutation): + frame, row = recorded_image() + row = deepcopy(row) + item = row["pnp_observation_evidence"]["child"] + if mutation == "sample_model": + item["image_model_sha256"] = "a" * 64 + elif mutation == "sample_corners": + item["corners_xy"] = ((0., 0.),) * 4 + elif mutation == "sample_pose": + item["selected_pose"]["translation_xyz_m"] = (.1, .1, .8) + elif mutation == "relative": + row["relative_translation_xyz_m"] = [0., 0., 0.] + elif mutation == "identity": + row["image_stamp_ns"] += 1 + else: + item["pose_source"] = "current_image" + item.pop("frozen_image_model") + item.pop("image_model_sha256") + with pytest.raises(ValueError, match="image_motion_provenance_"): + validate_image_sample(row, frame, replay_image_models(frame)) + + +def test_articulated_finalizer_replays_model_without_running_ippe_or_refitting(): + from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations + frame, row = recorded_image() + records = [] + # Distinct images with equal pose are sufficient for this read-back test; + # the separate motion initializer and final fitter enforce motion span. + for index in range(65): + current_frame, current_row = deepcopy(frame), deepcopy(row) + for record in (current_frame, current_row): + record.update(image_stamp_ns=100 + index, cycle=0 if index < 40 else 3) + for item in current_row["pnp_observation_evidence"].values(): + item["candidate_diagnostics"]["observation_stamp_ns"] = 100 + index + for item in current_frame["roles"].values(): + item["candidate_diagnostics"]["observation_stamp_ns"] = 100 + index + records.extend((current_frame, current_row)) + before = deepcopy(records) + result, report = resolve_chain_observations(records, task_key="task", role_pairs={"hinge": ("root", "moving")}, + fixed_roles={"root"}, require_frozen_branches=True, + solve_pose=lambda *args, **kwargs: pytest.fail("IPPE reselection would change frozen geometry")) + assert result == before == records + assert report["changed_joint_rows"] == 0 and report["geometry_refitted"] is False + assert report["holdout_frames"] == 25 + + +def image_motion_capture(): + """Construct a journal from an independently specified exact hinge model. + + Solver observability is tested in its own module. Here a known model and + images isolate read-back, authorization and zero/holdout identity handling. + """ + from linkerhand_calibration.core.domain.reference import build_joint_zero_reference + from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 + from test_motion_reference_provenance import motion_capture + + profile, old_reference, records = motion_capture(source_stamps=tuple(range(10, 34))) + name = old_reference.joint + spec = profile.measurement.measurements[name] + role_names = {"root": spec.parent_role, "moving": spec.child_role} + original_frame, original_sample = recorded_image() + raw_model = original_frame["roles"]["moving"]["frozen_image_model"] + raw_model["geometry"][0]["relation"] = dict(joint=name, parent_role=spec.parent_role, child_role=spec.child_role) + raw_model.update(tag_sizes=tuple((role_names[role], size) for role, size in raw_model["tag_sizes"]), + branches=tuple((role_names[role], branch) for role, branch in raw_model["branches"]), + source_stamps_ns=tuple(range(10, 34)), training_stamps_ns=tuple(range(10, 22)), + validation_stamps_ns=tuple(range(22, 34))) + digest = image_model_sha256(raw_model) + original_frame["roles"] = {role_names[role]: item for role, item in original_frame["roles"].items()} + moving = original_frame["roles"][spec.child_role] + moving.update(frozen_image_model=raw_model, image_model_sha256=digest) + for item in original_sample["pnp_observation_evidence"].values(): + item["tag_role"] = role_names[item["tag_role"]] + if item.get("frozen_image_model"): + item.update(frozen_image_model=raw_model, image_model_sha256=digest) + sources = [row for row in records if row["kind"] == "motion_branch_observation"] + for source in sources: + source.update(camera_matrix=MATRIX, input_is_rectified=True, sample_phase="zero_approach", + tags={role: {"tag_size_m": item["tag_size_m"], "corners_xy": item["corners_xy"], + "reprojection_valid_candidates": item["reprojection_valid_candidates"]} + for role, item in original_frame["roles"].items()}) + audit = next(row for row in records if row["kind"] == "motion_branch_initialization") + audit.update(constraints=[], frozen_image_model=raw_model, image_model_sha256=digest, + source_frame_hashes={str(row["image_stamp_ns"]): source_frame_sha256(row) for row in sources}) + payload = {key: audit[key] for key in ("task_name", "view", "zero_joints", "session_epoch", + "motion_version", "source_image_stamps", "source_frame_hashes", "constraints", + "frozen_image_model", "image_model_sha256")} + payload["tag_role"] = spec.child_role + evidence_id = image_model_sha256(payload) + audit.update(evidence_ids={spec.child_role: evidence_id}, evidence_payloads={spec.child_role: payload}) + output = [*sources, audit] + zero_rows = [] + for row in records: + if row["kind"] not in {"joint_zero_sample", "joint_sample"}: + continue + frame = deepcopy(original_frame) + for key in ("task_name", "view", "image_stamp_ns", "cycle", "direction", "attempt"): + frame[key] = row.get(key) + for item in frame["roles"].values(): + item["candidate_diagnostics"]["observation_stamp_ns"] = row["image_stamp_ns"] + row = deepcopy(row) + for field in ("pnp_observation_evidence", "parent_pose_common", "child_pose_common", + "relative_quaternion_xyzw", "relative_translation_xyz_m"): + row[field] = deepcopy(original_sample[field]) + for item in row["pnp_observation_evidence"].values(): + item["candidate_diagnostics"].update(observation_stamp_ns=row["image_stamp_ns"], + branch_frozen=row["kind"] != "joint_zero_sample", branch_revision=7) + if item["tag_role"] == spec.child_role: + item["candidate_diagnostics"].update(motion_evidence_verified=True, motion_evidence_id=evidence_id) + output.extend((frame, row)) + if row["kind"] == "joint_zero_sample": + zero_rows.append(row) + reference = build_joint_zero_reference(profile, name, zero_rows, session_epoch=1, motion_version=2) + output.append(reference.as_record()) + return profile, output + + +def test_new_model_reference_and_fourth_round_remain_bound_to_pre_zero_images(): + from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance + profile, records = image_motion_capture() + result = validate_motion_provenance(profile, records, require_complete=True) + assert result["motion_evidence_verified"] and not result["is_accuracy_certificate"] + + +def test_resume_preserves_zero_image_frames_for_later_provenance_replay(): + from linkerhand_calibration.core.domain.reference import read_joint_zero_references + from linkerhand_calibration.runtime.joint_resume import JointResume + from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance + + profile, records = image_motion_capture() + reference = next(iter(read_joint_zero_references(profile, records).values())) + zero_images = {(s.view, s.image_stamp_ns) for s in reference.samples} + expected = [r for r in records if r["kind"] == "pnp_candidate_frame" + and (r["view"], r["image_stamp_ns"]) in zero_images] + assert expected + for _ in range(2): + resume = JointResume(profile) + resume.references, resume.rows = {reference.joint: reference}, tuple(records) + # This test isolates transport of the frozen zero's original image + # evidence; independent new-pose verification is tested separately. + assert resume.verify(reference) is reference + copied = resume.take_motion_provenance_records() + assert [r for r in copied if r["kind"] == "pnp_candidate_frame"] == expected + records = [*copied, reference.as_record()] + assert validate_motion_provenance(profile, records, required=True)["motion_evidence_verified"] + + +@pytest.mark.parametrize("mutation", ["missing_frame", "source_changed", "holdout_changed", "old_model_disguised"]) +def test_final_provenance_does_not_accept_unreplayable_image_geometry(mutation): + from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance + profile, records = image_motion_capture() + if mutation == "missing_frame": + records = [row for row in records if row["kind"] != "pnp_candidate_frame"] + elif mutation == "source_changed": + records[0]["camera_matrix"] = ((1700., 0., 640.), *MATRIX[1:]) + else: + row = next(row for row in records if row.get("kind") == "joint_sample" and row.get("cycle") == 3) + item = row["pnp_observation_evidence"]["child"] + if mutation == "holdout_changed": + item["image_model_sha256"] = "a" * 64 + else: + item["pose_source"] = "current_image" + item.pop("frozen_image_model") + item.pop("image_model_sha256") + with pytest.raises(ValueError, match="provenance_"): + validate_motion_provenance(profile, records, require_complete=True) diff --git a/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py b/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py new file mode 100644 index 0000000..65281be --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py @@ -0,0 +1,150 @@ +"""The constrained-pose bridge verifies pixels once and commits metadata atomically.""" +from dataclasses import replace +from threading import Thread + +import pytest + +from linkerhand_calibration.core.geometry.tag_pose import tracking, image_motion_commit +from linkerhand_calibration.core.geometry.tag_pose.fixed_reference import FixedTagReference +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import RoleCandidates +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion +from linkerhand_calibration.core.geometry.tag_pose.tracking import MotionPoseSelection, SquareTagPoseTracker +from test_motion_image_diagnostics import frames, RELATIONS + + +@pytest.fixture(scope="module") +def model_source(): + source = frames(depth_error=.008) + result = resolve_image_motion(source, RELATIONS) + assert result.resolved + return result.model, source + + +@pytest.fixture +def observed(monkeypatch, model_source): + model, source = model_source + tracker = SquareTagPoseTracker() + root = source[0].evidence.roles[0].candidates[0] + root_observation = source[0].observations[0] + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [root]) + for index in range(5): + selected, _ = tracker.estimate("root", root_observation.corners_xy, tag_size_m=.016, + camera_matrix=source[0].camera_matrix, stamp_ns=100_000_000 + index * 50_000_000) + assert selected is not None + snapshot = tracker.motion_snapshot("root", 300_000_000) + assert tracker.freeze_roles({"root": snapshot.revision}) + reference = FixedTagReference(selected, snapshot.revision) + + def observe(frame): + for role, observation in zip(frame.evidence.roles, frame.observations): + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, _p=role.candidates, **k: list(_p)) + tracker.estimate(role.role, observation.corners_xy, tag_size_m=observation.tag_size_m, + camera_matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns, + fixed_reference=reference if role.role == "root" else None, + motion_managed=role.role != "root") + actual_root = tracker.motion_snapshot("root", frame.evidence.stamp_ns) + return replace(frame, evidence=replace(frame.evidence, roles=( + RoleCandidates("root", (actual_root.previous_pose,), True), frame.evidence.roles[1]))) + + frame = observe(source[12]) + return tracker, model, frame, observe, source + + +def verify(observed): + tracker, model, frame, _, _ = observed + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + return tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) + + +def test_constrained_pose_has_its_own_verified_commit_without_changing_pnp(observed, monkeypatch): + tracker, _, frame, _, _ = observed + original = tracker.last_candidates_by_role["moving"] + calls = [] + solve = image_motion_commit.select_image_motion_frame + def unlocked(model, frame): + completed = [] + thread = Thread(target=lambda: completed.append(tracker.motion_snapshot("moving", frame.evidence.stamp_ns))) + thread.start(); thread.join(timeout=1) + assert completed, "numerical image solve held the tracker lock" + calls.append(1) + return solve(model, frame) + monkeypatch.setattr(image_motion_commit, "select_image_motion_frame", unlocked) + decision = verify(observed) + assert decision.accepted, decision + assert decision.selections[0].pose not in original + legacy = tracker.commit_motion_frame(decision.selections, establish=True) + assert not legacy.accepted and legacy.reason == "pose_motion_selection_invalid" + result = tracker.commit_image_motion_frame(decision, establish=True) + assert result.accepted and len(calls) == 1 + assert tracker.last_candidates_by_role["moving"] == original + diagnostics = result.observation_snapshots[0].diagnostics + assert diagnostics["pose_source"] == "image_constrained_current_image" + assert diagnostics["frozen_image_model"]["policy"] == decision.model.policy + assert diagnostics["image_model_sha256"] == decision.model_sha256 + assert diagnostics["image_motion_quality"] + assert not tracker.commit_image_motion_frame(decision, establish=True).accepted + + +def test_copied_decision_cannot_authorize_an_arbitrary_pose(observed): + tracker, _, _, _, _ = observed + decision = verify(observed) + assert decision.accepted + pose = replace(decision.selections[0].pose, translation_xyz_m=(0., 0., .1)) + forged = replace(decision, selections=(replace(decision.selections[0], pose=pose),)) + assert tracker.commit_image_motion_frame(forged, establish=True).reason == "pose_image_motion_decision_invalid" + assert tracker.commit_image_motion_frame(decision, establish=True).accepted + + +def test_child_or_root_arriving_after_solve_invalidates_entire_decision(observed): + tracker, _, _, observe, source = observed + decision = verify(observed) + observe(source[13]) + result = tracker.commit_image_motion_frame(decision, establish=True) + assert not result.accepted and result.reason == "pose_observation_superseded" + assert tracker._branches.states["moving"].motion_evidence_id is None + + +def test_different_pixels_cannot_be_substituted_before_verification(observed): + tracker, model, frame, _, _ = observed + shifted = replace(frame.observations[1], corners_xy=tuple((x + .1, y) for x, y in frame.observations[1].corners_xy)) + frame = replace(frame, observations=(frame.observations[0], shifted)) + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + decision = tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) + assert not decision.accepted and decision.reason == "pose_image_motion_observation_mismatch" + + +def test_reset_and_freeze_cannot_adopt_a_delayed_model(observed): + tracker, _, _, _, _ = observed + decision = verify(observed) + tracker.reset() + assert tracker.commit_image_motion_frame(decision, establish=True).reason == "pose_observation_superseded" + + +def test_frozen_identity_cannot_change_model_or_return_to_legacy_solver(observed): + tracker, model, frame, observe, source = observed + decision = verify(observed) + assert tracker.commit_image_motion_frame(decision, establish=True).accepted + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + assert tracker.freeze_roles({"moving": snapshot.revision}, require_motion_evidence=True) + frame = observe(source[13]) + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + legacy = MotionPoseSelection(snapshot, snapshot.candidates[0], "evidence") + assert tracker.commit_motion_frame((legacy,)).reason == "pose_motion_solver_changed" + changed = replace(model, source_stamps_ns=(*model.source_stamps_ns, model.source_stamps_ns[-1] + 1)) + decision = tracker.verify_image_motion_frame((snapshot,), changed, frame, {"moving": "evidence"}) + assert decision.accepted + assert tracker.commit_image_motion_frame(decision).reason == "pose_image_motion_model_changed" + decision = tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) + assert tracker.commit_image_motion_frame(decision).accepted + + +def test_only_the_fixed_root_advancing_also_invalidates_decision(observed, monkeypatch): + tracker, _, frame, _, _ = observed + decision = verify(observed) + root = tracker.motion_snapshot("root", frame.evidence.stamp_ns) + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [root.previous_pose]) + tracker.estimate("root", frame.observations[0].corners_xy, tag_size_m=.016, + camera_matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns + 1, + fixed_reference=FixedTagReference(root.previous_pose, root.revision)) + assert tracker.commit_image_motion_frame(decision, establish=True).reason == "pose_observation_superseded" + assert tracker._branches.states["moving"].motion_evidence_id is None diff --git a/src/linkerhand_calibration/test/test_image_projection_worker.py b/src/linkerhand_calibration/test/test_image_projection_worker.py new file mode 100644 index 0000000..b8de98d --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_projection_worker.py @@ -0,0 +1,53 @@ +"""Independent execution preserves current pixels, geometry and rejection.""" + +from dataclasses import replace +from types import SimpleNamespace + +from linkerhand_calibration.core.geometry.tag_pose.image_projection_worker import ImageProjectionWorker +from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict +from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame +from test_image_motion_provenance import recorded_image +from test_motion_image_diagnostics import frames +from test_image_motion_tracker_commit import observed, model_source + + +def test_process_projection_matches_direct_solution_and_rejects_changed_camera(): + recorded, _ = recorded_image() + model = image_motion_model_from_dict(recorded["roles"]["moving"]["frozen_image_model"]) + frame = frames(depth_error=.008)[24] + worker = ImageProjectionWorker(workers=1) + try: + actual = worker.solve(model, frame) + assert actual.resolved + expected = select_image_motion_frame(model, frame) + assert actual == expected + changed = replace(frame, camera_matrix=((1700., 0., 640.), *frame.camera_matrix[1:])) + assert worker.solve(model, changed).reason == "image_motion_camera_changed" + finally: + worker.close() + assert not worker.solve(model, frame).resolved + + +def test_process_result_still_requires_current_tracker_identity(observed): + tracker, model, frame, observe, source = observed + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + worker = ImageProjectionWorker(workers=1) + try: + decision = tracker.verify_image_motion_frame((snapshot,), model, frame, + {"moving": "evidence"}, projection_worker=worker) + assert decision.accepted + observe(source[13]) + result = tracker.commit_image_motion_frame(decision, establish=True) + assert not result.accepted and result.reason == "pose_observation_superseded" + assert tracker._branches.states["moving"].motion_evidence_id is None + finally: + worker.close() + + +def test_caller_cannot_substitute_a_pose_proposal_as_a_worker(observed): + tracker, model, frame, _, _ = observed + snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) + forged = SimpleNamespace(solve=lambda *_: select_image_motion_frame(model, frame)) + decision = tracker.verify_image_motion_frame((snapshot,), model, frame, + {"moving": "evidence"}, projection_worker=forged) + assert not decision.accepted and decision.reason == "pose_image_motion_worker_invalid" diff --git a/src/linkerhand_calibration/test/test_independent_urdf_export.py b/src/linkerhand_calibration/test/test_independent_urdf_export.py new file mode 100644 index 0000000..a238f78 --- /dev/null +++ b/src/linkerhand_calibration/test/test_independent_urdf_export.py @@ -0,0 +1,206 @@ +"""A near-zero limit must never flatten a nonlinear passive joint's travel.""" + +from copy import deepcopy +import hashlib +import json +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.patch import UrdfJointPatch, UrdfPatchSet, write_urdf_patches +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.core.urdf.validate import validate_structural_urdf_diff +from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( + rebuild_urdf_from_json, serialize_urdf_correction, validate_json_urdf_correspondence, +) + + +@pytest.fixture +def nonlinear_release(tmp_path): + source = tmp_path / "original.urdf" + source.write_text(''' + + + + + + + +''') + digest = hashlib.sha256(source.read_bytes()).hexdigest() + rights = {name: ("origin.rpy", "limit.lower", "limit.upper", "mimic.multiplier", "mimic.offset") + for name in ("pitch", "ip")} + plan = build_standard_correction_plan(source_urdf=source, source_sha256=digest, + zero_offsets_rad={}, authorized_fields=rights, independent_joint_channels={"pitch": 0, "ip": 0}) + parent = np.linspace(0, .6, 256) + child = 1.71 * parent + .39 * parent**2 + payload = {"format": "unified_calibration_v3", "schema_version": 3, + "profile_id": "VIRTUAL/right/nonlinear/v3", "input_unit": "u8", "baseline_command": [0], + "joints": {name: {"sdk_channel": 0, "angle_rad": angles.tolist()} + for name, angles in (("pitch", parent), ("ip", child))}, + "urdf_correction": serialize_urdf_correction(plan)} + path = tmp_path / "calibration.json" + path.write_text(json.dumps(payload)) + return source, rights, plan, payload, path + + +def rebuild(data, output): + source, rights, _, payload, path = data + path.write_text(json.dumps(payload)) + return rebuild_urdf_from_json(path, source, output, authorized_fields=rights, + expected_profile_id="VIRTUAL/right/nonlinear/v3") + + +def test_nonlinear_angles_survive_normal_urdf_fk_and_json_rebuild(nonlinear_release, tmp_path): + source, _, plan, payload, _ = nonlinear_release + original = source.read_bytes() + output = rebuild(nonlinear_release, tmp_path / "corrected.urdf") + assert output.read_bytes() == plan.write(source, tmp_path / "direct.urdf").read_bytes() + validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) + model = UrdfKinematicModel(output) + mapping = SerializedJointMapping(payload, input_kind="command") + ratios = [] + for command in (32, 128, 255): + angles = mapping.evaluate([command], "", active_joints=("pitch", "ip")) + resolved = model.resolve_angles(angles) # No custom mimic override. + assert resolved == angles + ratios.append(resolved["ip"] / resolved["pitch"]) + expected = np.eye(4) + for name in ("pitch", "ip"): + joint = model.joints[name] + expected = expected @ joint.origin @ model._motion(joint, angles[name]) + assert np.allclose(model.link_transform("ip", zero_offsets={}, joint_angles=angles), expected) + assert ratios[-1] - ratios[0] > .2 + assert np.degrees(resolved["ip"]) > 66 + assert source.read_bytes() == original + for name, joint in model.joints.items(): + before = UrdfKinematicModel(source).joints[name] + assert joint.lower == before.lower and joint.upper == before.upper + assert np.array_equal(joint.origin, before.origin) + assert ET.parse(output).findall("./joint/mimic") == [] + + +@pytest.mark.parametrize("corruption", ["wrong_channel", "missing_curve", "wrong_parent", "missing_removal", + "legacy_removal", "coefficient_and_removal", "extra_removal", "changed_axis"]) +def test_incomplete_or_changed_coupling_cannot_rebuild(nonlinear_release, tmp_path, corruption): + source, rights, plan, original, path = nonlinear_release + payload = deepcopy(original) + correction = payload["urdf_correction"] + if corruption == "wrong_channel": + payload["joints"]["ip"]["sdk_channel"] = 1 + payload["baseline_command"].append(0) + elif corruption == "missing_curve": + del payload["joints"]["ip"] + elif corruption == "wrong_parent": + correction["passive_joint_sources"]["ip"] = "missing" + elif corruption == "missing_removal": + correction["joint_patches"]["ip"] = {"limit_upper": "1.2"} + elif corruption == "legacy_removal": + correction["schema_version"] = 1 + del correction["joint_angle_source"], correction["passive_joint_sources"] + elif corruption == "coefficient_and_removal": + correction["joint_patches"]["ip"]["mimic_multiplier"] = "2" + elif corruption == "extra_removal": + correction["joint_patches"]["pitch"] = {"remove_mimic": True} + else: + correction["joint_patches"]["ip"]["axis_xyz"] = "1 0 0" + output = tmp_path / "invalid.urdf" + with pytest.raises(ValueError): + rebuild((source, rights, plan, payload, path), output) + assert not output.exists() + + +def test_removal_requires_its_own_right_and_preserves_linkage(nonlinear_release, tmp_path): + source, rights, plan, payload, _ = nonlinear_release + with pytest.raises(ValueError, match="authorization"): + write_urdf_patches(source_urdf=source, destination_urdf=tmp_path / "unauthorized.urdf", + patches=UrdfPatchSet({"ip": UrdfJointPatch(remove_mimic=True)}), authorized_fields=rights) + output = rebuild(nonlinear_release, tmp_path / "valid.urdf") + changed = output.read_text().replace('parent link="proximal"', 'parent link="base"') + output.write_text(changed) + with pytest.raises(ValueError, match="topology"): + validate_structural_urdf_diff(source, output, authorized_fields=plan.authorized_fields) + with pytest.raises(ValueError, match="overridden"): + validate_compact_urdf_tables(payload, UrdfKinematicModel(source)) + + +def test_robot_state_publisher_preserves_json_passive_angles(nonlinear_release, tmp_path): + assert_robot_state_publisher_angles(nonlinear_release, tmp_path) + + +def assert_robot_state_publisher_angles(nonlinear_release, tmp_path, *, use_mimic=False): + """Use private topics, real ROS TF and no SDK or motor-command publisher.""" + rclpy = pytest.importorskip("rclpy") + from ament_index_python.packages import get_package_prefix, PackageNotFoundError + from pathlib import Path + import os + import subprocess + import time + import uuid + import yaml + from scipy.spatial.transform import Rotation + from sensor_msgs.msg import JointState + from tf2_msgs.msg import TFMessage + try: + executable = Path(get_package_prefix("robot_state_publisher")) / "lib/robot_state_publisher/robot_state_publisher" + except PackageNotFoundError: + pytest.skip("robot_state_publisher is not installed") + output = rebuild(nonlinear_release, tmp_path / "ros.urdf") + model = UrdfKinematicModel(output) + parameters = tmp_path / "parameters.yaml" + parameters.write_text(yaml.safe_dump({"/**": {"ros__parameters": { + "robot_description": output.read_text(), "publish_frequency": 100.0, "ignore_timestamp": True}}})) + namespace = "/nonlinear_export_test_" + uuid.uuid4().hex + context = rclpy.context.Context() + rclpy.init(context=context) + node = rclpy.create_node("json_angle_test", namespace=namespace, context=context) + executor = rclpy.executors.SingleThreadedExecutor(context=context) + executor.add_node(node) + messages = [] + subscription = node.create_subscription(TFMessage, namespace + "/tf", messages.append, 10) + publisher = node.create_publisher(JointState, namespace + "/joint_states", 10) + process = subprocess.Popen([str(executable), "--ros-args", "--params-file", str(parameters), + "-r", f"__ns:={namespace}", "-r", f"/tf:={namespace}/tf", + "-r", f"/tf_static:={namespace}/tf_static"], stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, env=dict(os.environ, ROS_LOG_DIR=str(tmp_path))) + try: + for command in (32, 128, 255): + payload = nonlinear_release[3] + angles = [payload["joints"][name]["angle_rad"][command] for name in ("pitch", "ip")] + state = JointState() + state.header.stamp = node.get_clock().now().to_msg() + state.name, state.position = (["pitch"], angles[:1]) if use_mimic else (["pitch", "ip"], angles) + messages.clear() + deadline = time.monotonic() + 8 + observed = None + while time.monotonic() < deadline: + assert process.poll() is None + publisher.publish(state) + executor.spin_once(timeout_sec=.05) + for message in messages: + for transform in message.transforms: + if transform.child_frame_id == "distal" and transform.header.stamp == state.header.stamp: + observed = transform + if observed is not None: + break + assert observed is not None, "no passive-joint TF from robot_state_publisher" + assert observed.header.frame_id == "proximal" + q = observed.transform.rotation + rotation = Rotation.from_quat([q.x, q.y, q.z, q.w]) + expected_angle = model.resolve_angles({"pitch": angles[0]})["ip"] if use_mimic else angles[1] + expected = Rotation.from_rotvec([0, 0, expected_angle]) + assert (rotation.inv() * expected).magnitude() < 1e-10 + finally: + process.terminate() + try: + process.wait(timeout=2) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=2) + executor.shutdown() + node.destroy_subscription(subscription) + node.destroy_node() + context.shutdown() diff --git a/src/linkerhand_calibration/test/test_joint_zero_runtime.py b/src/linkerhand_calibration/test/test_joint_zero_runtime.py index c93c7e3..9d19765 100644 --- a/src/linkerhand_calibration/test/test_joint_zero_runtime.py +++ b/src/linkerhand_calibration/test/test_joint_zero_runtime.py @@ -7,6 +7,7 @@ import pytest from linkerhand_calibration.profiles import load_bundled_hand_profile from linkerhand_calibration.core.domain.reference import build_joint_zero_reference, read_joint_zero_references from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION from linkerhand_calibration.runtime.session import CalibrationPhase as Phase from linkerhand_calibration.runtime.scan_quality import observation_streams from linkerhand_calibration.runtime.joint_resume import JointResume @@ -50,12 +51,27 @@ def reference_rows(profile, name, *, stamp=1, tilt=0): pose = {"quaternion_xyzw": [0, 0, 0, 1], "translation_xyz_m": [0, 0, .5]} child = {"quaternion_xyzw": Rotation.from_rotvec([tilt, 0, 0]).as_quat().tolist(), "translation_xyz_m": [0, 0, .6]} - return [{"view": view, "image_stamp_ns": stamp+i, + rows = [{"view": view, "image_stamp_ns": stamp+i, "relative_quaternion_xyzw": child["quaternion_xyzw"], "relative_translation_xyz_m": [0, 0, .1], f"command_vector_{profile.command.unit}": spec.command, f"state_{profile.command.unit}": spec.command, "command_direction_by_index": [direction]*profile.command.command_count, "parent_pose_common": pose, "child_pose_common": child} for view in views for i in range(profile.acquisition.fixed_reference_minimum_frames)] + for row in rows: + primary = profile.measurement.measurements[name] + measurement = primary if primary.view == row["view"] else profile.measurement.measurements[secondary] + row["pnp_observation_evidence"] = { + side: {"pose_source": "current_image", "tag_role": role, + "candidate_diagnostics": {"observation_stamp_ns": row["image_stamp_ns"], + "branch_revision": 0, "branch_status": "tracking"}} + for side, role in (("parent", measurement.parent_role), ("child", measurement.child_role))} + return rows + + +def sweep_feedback_values(start, target, unit): + """Sample only this motion segment, at the same density over a full range.""" + fraction = abs(target-start)/abs(unit.end-unit.start) + return np.linspace(start, target, max(2, int(np.ceil(128*fraction))+1)) @pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) @@ -79,6 +95,7 @@ def test_deferred_zero_is_locked_once_before_own_training_in_every_model(layout) if phase == Phase.EVALUATE: assert driver.evaluate(rows).passed continue + segment_start = command motion = driver.motion(command) command = motion.target trace.append((motion.phase, motion.task_key, motion.cycle)) @@ -96,14 +113,17 @@ def test_deferred_zero_is_locked_once_before_own_training_in_every_model(layout) task = next(t for t in profile.motion.tasks if t.key == motion.task_key) assert set(task.joints) <= references.keys() unit = driver.action.scan_unit + feedback_values = (sweep_feedback_values(segment_start[task.command_index], + command[task.command_index], unit) if motion.phase == "sweep" else + (command[task.command_index],)*profile.acquisition.steady_minimum_samples) for field, name, spec in observation_streams(profile, task): - for i in range(129 if motion.phase == "sweep" else 3): + for feedback in feedback_values: stamp += 1 rows.append({field: name, "view": spec.view, "task_name": task.key, "cycle": unit.cycle, "direction": unit.direction, "attempt": 1, "image_stamp_ns": stamp, "sample_phase": motion.phase, "steady_index": motion.steady_index, "steady_target": command[task.command_index], - f"feedback_{profile.command.unit}": unit.start+(unit.end-unit.start)*i/128}) + f"feedback_{profile.command.unit}": float(feedback)}) driver.motion_complete() assert driver.session.phase == Phase.FIT assert references == driver.zero_references @@ -138,6 +158,30 @@ def test_resume_verifies_same_pose_and_preserves_original_zero(): read_joint_zero_references(profile, [tampered]) +def test_incomplete_zero_window_does_not_revalidate_poses_on_every_control_tick(monkeypatch): + from linkerhand_calibration.core.domain.reference import JointZeroSample + profile = load_bundled_hand_profile("o6_right_8") + name = next(iter(profile.motion.joint_zero_references)) + rows = reference_rows(profile, name) + calls = [] + original = JointZeroSample.from_row + + def counted(row, unit): + calls.append(row["image_stamp_ns"]) + return original(row, unit) + + monkeypatch.setattr(JointZeroSample, "from_row", staticmethod(counted)) + for _ in range(20): + with pytest.raises(ValueError, match="joint_zero_samples_missing"): + build_joint_zero_reference(profile, name, rows[:-1], session_epoch=1, motion_version=1) + assert not calls + build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=1) + assert len(calls) == len(rows) + rows[0]["relative_translation_xyz_m"] = [9, 9, 9] + with pytest.raises(ValueError, match="joint_zero_relative_pose_inconsistent"): + build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=1) + + def test_same_joint_in_later_task_cannot_acquire_a_second_reference(): profile = load_bundled_hand_profile("o6_right_8") first = profile.motion.tasks[0] @@ -155,6 +199,9 @@ def test_same_joint_in_later_task_cannot_acquire_a_second_reference(): def test_native_resume_imports_training_travel_only_after_reference_verification(tmp_path): + from copy import deepcopy + from pose_branch_fixture import add_ideal_motion_provenance + from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION from runtime_host_fixture import coordinator_fixture, ready from linkerhand_calibration.runtime.steady import steady_targets host, clock = coordinator_fixture(tmp_path, model="o12") @@ -162,27 +209,46 @@ def test_native_resume_imports_training_travel_only_after_reference_verification profile = host.profile task = profile.motion.tasks[0] units = [u for u in host.execution.session.engine.scan_units() if u.task_key == task.key and u.cycle == 0] - records, stamp = [], 100 + records, stamp = [{"kind": "session_start", "profile_id": profile.key.profile_id, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": host.execution.session.engine.capture_schedule_version, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION}], 100 + templates = {} for name in task.joints: - reference = build_joint_zero_reference(profile, name, reference_rows(profile, name), session_epoch=1, motion_version=1) + zeros = reference_rows(profile, name, stamp=10) + records.extend(add_ideal_motion_provenance(profile, name, zeros, + session_epoch=1, motion_version=1, source_stamps=(1, 2))) + templates[name] = {row["view"]: row for row in zeros} + reference = build_joint_zero_reference(profile, name, zeros, session_epoch=1, motion_version=2) records.append(reference.as_record()) for unit in units: - for field, name, spec in observation_streams(profile, task): - for phase, targets in (("sweep", np.linspace(unit.start, unit.end, 129)), - ("steady", steady_targets(profile, unit))): - for index, target in enumerate(targets): - for _ in range(1 if phase == "sweep" else 3): + previous = unit.start + for index, target in enumerate(steady_targets(profile, unit)): + phases = [] + if index: + phases.append(("sweep", sweep_feedback_values(previous, target, unit))) + phases.append(("steady", (target,)*profile.acquisition.steady_minimum_samples)) + for phase, feedback_values in phases: + for field, name, spec in observation_streams(profile, task): + for feedback in feedback_values: stamp += 1 + evidence = deepcopy(templates[name][spec.view]["pnp_observation_evidence"]) + for item in evidence.values(): + item["candidate_diagnostics"].update(observation_stamp_ns=stamp, branch_frozen=True) records.append({"kind": "joint_sample" if field == "joint" else "secondary_joint_sample", field: name, "view": spec.view, "task_name": task.key, "cycle": 0, "direction": unit.direction, "attempt": 1, "image_stamp_ns": stamp, "sample_phase": phase, "steady_index": index, "steady_target": target, - "feedback_rad": float(target)}) + "feedback_rad": float(feedback), "pnp_observation_evidence": evidence}) + previous = target keys = {(u.task_key, u.cycle, u.direction) for u in units} host.joint_resume.stage(host.execution.session.engine, records, keys) assert host.joint_resume.take_task(task.key) == ((), ()) for name in task.joints: - current = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=2000), + zeros = reference_rows(profile, name, stamp=2000) + add_ideal_motion_provenance(profile, name, zeros, + session_epoch=2, motion_version=2, source_stamps=(1900, 1901)) + current = build_joint_zero_reference(profile, name, zeros, session_epoch=2, motion_version=3) host.execution.zero_references[name] = host.joint_resume.verify(current) ready(host, clock) @@ -192,6 +258,10 @@ def test_native_resume_imports_training_travel_only_after_reference_verification host.last_command = profile.command.baseline_values host.execution.session.phase = Phase.PREPARE host.tick() + while host._resume_import is not None: + clock.now += .02 + host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns()) + host.tick() assert host._resumed_count == 2 and host.execution.session.current_unit.cycle == 1 assert host.execution.first_cycle_spans assert all(value == pytest.approx(1) for value in host.execution.first_cycle_spans.values()) @@ -207,9 +277,12 @@ def test_zero_capture_recovers_after_transient_bad_pose_without_loosening_stabil name = next(iter(host.profile.motion.joint_zero_references)) bad = reference_rows(host.profile, name, tilt=.1)[:1] stable = reference_rows(host.profile, name, stamp=100) + from pose_branch_fixture import confirm_reference_rows + confirm_reference_rows(host, stable) host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in bad+stable] host._observation_epoch = host._segment_number = 1 - assert host._lock_joint_zeros(SimpleNamespace(zero_joints=(name,))) + assert host._lock_joint_zeros(SimpleNamespace(task_key=host.profile.motion.joint_zero_references[name].task_key, + zero_joints=(name,))) reference = host.execution.zero_references[name] assert all(sample.image_stamp_ns >= 100 for sample in reference.samples) assert len(host._steady_rows) == len(bad)+len(stable) # Evidence was not erased. diff --git a/src/linkerhand_calibration/test/test_joint_zero_timing.py b/src/linkerhand_calibration/test/test_joint_zero_timing.py new file mode 100644 index 0000000..6f0630a --- /dev/null +++ b/src/linkerhand_calibration/test/test_joint_zero_timing.py @@ -0,0 +1,102 @@ +"""Irregular control timing must not erase stable joint-zero observations.""" + +import numpy as np +import pytest + +from linkerhand_calibration.runtime.inputs import DetectionInput +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready +from test_joint_zero_runtime import reference_rows +from pose_branch_fixture import assume_motion_confirmation, confirm_reference_rows + + +@pytest.mark.parametrize("feedback_moves,fixed_occluded,recovery", [ + (False, False, False), (True, False, False), (False, True, False), (False, False, True), + (False, False, "slow"), (False, False, "nine_only")], + ids=["timer_jitter", "real_movement", "persistent_fixed_occlusion", "post_geometry_recovery", + "ten_slow_images", "nine_images_never_complete"]) +def test_zero_capture_requires_stable_feedback_and_valid_reference_images( + tmp_path, monkeypatch, feedback_moves, fixed_occluded, recovery): + host, clock = coordinator_fixture(tmp_path, model="o6") + try: + ready(host, clock) + assert host.start().success + task = host.profile.motion.tasks[0] + name = task.joints[0] + spec = host.profile.motion.joint_zero_references[name] + host.execution.session.phase = Phase.PREPARE + host.last_command = spec.command + # The declared approach has completed; exercise the real zero action, + # coordinator timing gates and reference builder from this point. + host.execution.motion(spec.command) + effects = host.execution._effects + index = next(i for i, effect in enumerate(effects) if effect.phase == "joint_zero") + host.execution._effects = effects[index:] + host.commanded_speed = host.execution._effects[0].speed + host._command_directions = ["increasing"]*host.command_count + assume_motion_confirmation(host, task.joints) + + def consume(frame, motion, **kwargs): + if motion is None: + return (), None + assert frame.feedback is not None and frame.command == spec.command + if fixed_occluded: + return ({"kind": "pnp_branch_event", "view": "top", "tag_role": "top_base", "tag_id": 6, + "image_stamp_ns": frame.stamp_ns, "reason": "fixed_reference_pose_unverified", + "pnp_observation_evidence": {"candidate_diagnostics": { + "observation_stamp_ns": frame.stamp_ns, "branch_status": "fixed_reference_unavailable", + "pose_constraint": "fixed_reference", "fixed_reference_verified": False}}},), None + row = reference_rows(host.profile, name, stamp=frame.stamp_ns)[0] + confirm_reference_rows(host, [row]) + return (dict(row, kind="joint_zero_sample", joint=name),), None + + monkeypatch.setattr(host.capture, "consume", consume) + start = clock.now + frames = (.59, .68, .77, .79, .81, .83, .85, .87, .89, .91) + if recovery: + # Real recorded run: tracker recovery consumes the first part of + # the post-solve window, followed by valid images at about 7 Hz. + frames = tuple(1.1+.14*i for i in range(10)) + if recovery in {"slow", "nine_only"}: + frames = tuple(0.8+.35*i for i in range(9 if recovery == "nine_only" else 10)) + times = sorted({*np.arange(0, .51, .02), *frames, frames[-1]+.02, .93, 2.02, 3.5, 7.1}) + if recovery in {"slow", "nine_only"}: + # A failed zero image window now gets one bounded fresh window. + # Keep feedback alive through that window without adding images. + times = sorted({*times, *np.arange(0, 9.11, .02)}) + for elapsed in times: + clock.now = start+float(elapsed) + feedback = list(spec.command) + if feedback_moves and elapsed == .77: + feedback[task.command_index] -= 5 + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + host.tick() + if recovery and elapsed == 0: + host._zero_geometry_completed_at = clock.now + if name in host.execution.zero_references: + break + if elapsed in frames: + host.receive_detections(DetectionInput(task.view, host.ports.clock_ns(), ())) + + if feedback_moves or fixed_occluded or recovery == "nine_only": + assert not host.execution.zero_references + assert host.execution.session.phase == Phase.PAUSED + assert "joint_zero_unavailable" in host.reason + assert len([r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"]) == 1 + if fixed_occluded: + assert "fixed_reference_pose_unverified" in host.reason + assert "top:tag=6" in host.reason + assert not host.finalization.started + else: + assert host.execution.session.phase != Phase.PAUSED, host.reason + reference = host.execution.zero_references[name] + assert len(reference.samples) == host.profile.acquisition.fixed_reference_minimum_frames == 10 + assert len([r for r in host.raw_records if r["kind"] == "joint_zero_reference"]) == 1 + if recovery == "slow": + assert host.profile.acquisition.steady_timeout_seconds < clock.now-start < 5.0 + elif recovery: + assert host.profile.acquisition.steady_timeout_seconds < clock.now-start < 3.0 + else: + assert clock.now-start < host.profile.acquisition.steady_timeout_seconds + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_json_urdf_rebuild.py b/src/linkerhand_calibration/test/test_json_urdf_rebuild.py new file mode 100644 index 0000000..1eb1f08 --- /dev/null +++ b/src/linkerhand_calibration/test/test_json_urdf_rebuild.py @@ -0,0 +1,92 @@ +"""JSON alone reproduces the written URDF and rejects unsafe edits atomically.""" + +import hashlib +import json + +import numpy as np +import pytest + +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( + rebuild_urdf_from_json, serialize_urdf_correction, + validate_json_urdf_correspondence, +) + + +@pytest.fixture +def saved_calibration(tmp_path): + source = tmp_path/"source.urdf" + source.write_text(''' + + + + + + + +''') + rights = {name: ("origin.rpy", "limit.lower", "limit.upper", "mimic.multiplier", "mimic.offset") + for name in ("drive", "tip")} + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad={"drive": .1, "tip": .05}, + measured_ranges_output_rad={"drive": (0., 1.5), "tip": (0., .75)}, + authorized_fields=rights) + payload = {"format": "unified_calibration_v3", "schema_version": 3, + "profile_id": "VIRTUAL/right/rebuild/v1", "input_unit": "u8", "baseline_command": [0], + "joints": {name: {"sdk_channel": 0, "angle_rad": np.linspace(0, upper, 256).tolist()} + for name, upper in (("drive", 1.5), ("tip", .75))}, + "urdf_correction": serialize_urdf_correction(plan)} + path = tmp_path/"calibration.json" + path.write_text(json.dumps(payload)) + return source, rights, plan, path, payload + + +def rebuild(saved, output): + source, rights, _, path, payload = saved + return rebuild_urdf_from_json(path, source, output, + authorized_fields=rights, expected_profile_id=payload["profile_id"]) + + +def test_saved_json_reproduces_bytes_and_is_the_only_edit_source(saved_calibration, tmp_path): + source, _, plan, path, payload = saved_calibration + digest = hashlib.sha256(source.read_bytes()).hexdigest() + expected = plan.write(source, tmp_path/"expected.urdf") + output = rebuild(saved_calibration, tmp_path/"from_json.urdf") + assert output.read_bytes() == expected.read_bytes() + validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) + # A later caller has no in-memory fit. The requested edit is read anew. + payload["urdf_correction"]["joint_patches"]["drive"]["origin_rpy"] = "0 0 0.2" + path.write_text(json.dumps(payload)) + with pytest.raises(ValueError, match="correction_differs_from_urdf"): + validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) + changed = rebuild(saved_calibration, tmp_path/"changed.urdf") + assert not np.allclose(UrdfKinematicModel(changed).joints["drive"].origin, + UrdfKinematicModel(expected).joints["drive"].origin) + assert hashlib.sha256(source.read_bytes()).hexdigest() == digest + with pytest.raises(ValueError, match="overwrite"): + rebuild(saved_calibration, output) + + +@pytest.mark.parametrize("corruption", ["source", "profile", "unknown_field", "nonfinite", "curve_limit", "missing"]) +def test_invalid_json_never_installs_a_destination(saved_calibration, tmp_path, corruption): + source, rights, _, path, payload = saved_calibration + if corruption == "source": + source.write_text(source.read_text()+"\n") + elif corruption == "profile": + payload["profile_id"] = "WRONG" + elif corruption == "unknown_field": + payload["urdf_correction"]["joint_patches"]["drive"]["axis_xyz"] = "1 0 0" + elif corruption == "nonfinite": + payload["urdf_correction"]["joint_patches"]["drive"]["origin_rpy"] = "0 nan 0" + elif corruption == "curve_limit": + payload["urdf_correction"]["joint_patches"]["drive"]["limit_upper"] = "1.4" + else: + del payload["urdf_correction"] + path.write_text(json.dumps(payload)) + output = tmp_path/"invalid.urdf" + with pytest.raises(ValueError): + rebuild_urdf_from_json(path, source, output, authorized_fields=rights, + expected_profile_id="VIRTUAL/right/rebuild/v1") + assert not output.exists() diff --git a/src/linkerhand_calibration/test/test_mapping_input_contract.py b/src/linkerhand_calibration/test/test_mapping_input_contract.py new file mode 100644 index 0000000..ded9946 --- /dev/null +++ b/src/linkerhand_calibration/test/test_mapping_input_contract.py @@ -0,0 +1,74 @@ +"""Timed feedback remains continuous across fitting and serialized replay.""" + +import math + +import pytest + +from linkerhand_calibration.acquisition import StateSample, interpolate_state_u8 +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping +from linkerhand_calibration.runtime.artifacts.serializers.unified_v1 import _mapping + + +def mapping(domain): + return JointMapping("joint", 0, domain, (0., 100., 255.), + (0., 1., 4.), (0., 1.2, 4.), (0., .8, 4.)) + + +def serialized(curve, version=3): + kind, unit = curve.input_domain.split("_") + payload = { + "format": "unified_calibration_report_v3" if version == 3 else "unified_calibration_v1", + "schema_version": version, "command_unit": unit, "command_names": ["motor"], + "joints": {"joint": {"urdf_joint": "joint", "passive": False, + kind + "_to_rad": _mapping(curve)}}, + } + return SerializedJointMapping(payload, input_kind=kind) + + +@pytest.mark.parametrize("version", [1, 3]) +@pytest.mark.parametrize("direction, at_100", [("", 1.), ("increasing", 1.2), ("decreasing", .8)]) +def test_fractional_feedback_agrees_with_declared_piecewise_linear_curve(version, direction, at_100): + curve = mapping("feedback_u8") + reader = serialized(curve, version) + # A half-byte measured position must not acquire a new quantization jump. + for value in (99.49, 99.50, 99.51): + expected = value / 100. * at_100 + assert curve.evaluate(value, direction) == pytest.approx(expected) + assert reader.evaluate((value,), direction, active_joints=("joint",))["joint"] == pytest.approx(expected) + + +def test_image_between_two_can_receipts_retains_its_interpolated_position(): + state, _ = interpolate_state_u8( + [StateSample(1_000_000, (99.,)), StateSample(3_000_000, (100.,))], + 2_000_000, maximum_skew_ns=2_000_000) + assert state == (99.5,) + reader = serialized(mapping("feedback_u8")) + assert reader.evaluate(state, "increasing", active_joints=("joint",))["joint"] == pytest.approx(1.194) + + +@pytest.mark.parametrize("domain", ["command_u8", "command_rad", "feedback_rad"]) +def test_wire_commands_and_radian_inputs_keep_their_existing_semantics(domain): + curve = mapping(domain) + value = 99.6 + expected_input = 100. if domain == "command_u8" else value + expected = expected_input / 100. + assert curve.evaluate(value) == pytest.approx(expected) + assert serialized(curve).evaluate((value,), "", active_joints=("joint",))["joint"] == pytest.approx(expected) + + +@pytest.mark.parametrize("domain", ["command_u8", "feedback_u8"]) +@pytest.mark.parametrize("value", [-.1, 255.1, math.nan]) +def test_quantization_cannot_hide_an_input_outside_measured_support(domain, value): + curve = mapping(domain) + with pytest.raises(ValueError): + curve.evaluate(value) + with pytest.raises(ValueError): + serialized(curve).evaluate((value,), "", active_joints=("joint",)) + + +@pytest.mark.parametrize("domain", ["command_u8", "feedback_u8"]) +def test_legacy_byte_table_keeps_its_deployed_lookup(domain): + reader = SerializedJointMapping({"schema_version": 6, "curve_input_domain": domain, + "joints": {"joint": {"motor_index": 0, "angle_rad": list(range(256))}}}) + assert reader.evaluate((99.6,), "", active_joints=("joint",))["joint"] == 100. diff --git a/src/linkerhand_calibration/test/test_measured_feedback_time.py b/src/linkerhand_calibration/test/test_measured_feedback_time.py new file mode 100644 index 0000000..8cbae77 --- /dev/null +++ b/src/linkerhand_calibration/test/test_measured_feedback_time.py @@ -0,0 +1,68 @@ +"""Feedback synchronization uses each physical receipt, never a repeated cache.""" + +import json +import pytest + +from linkerhand_calibration.acquisition import StateSample, interpolate_state_u8 +from linkerhand_calibration.runtime.feedback import decode_timed_feedback +from runtime_host_fixture import coordinator_fixture + + +def test_independent_channel_times_recover_a_linear_motion_without_cache_delay(): + samples = ( + StateSample(10_000_000, (0., 2.), (1_000_000, 10_000_000)), + StateSample(20_000_000, (10., 2.), (20_000_000, 10_000_000)), + StateSample(30_000_000, (10., 6.), (20_000_000, 30_000_000)), + ) + values, skew = interpolate_state_u8(samples, 15_000_000, maximum_skew_ns=50_000_000) + assert values == pytest.approx((140/19, 3.)) + assert skew == 5_000_000 + + +def test_recent_cache_cannot_hide_a_stale_channel(): + samples = (StateSample(100_000_000, (10., 20.), (1_000_000, 100_000_000)),) + assert interpolate_state_u8(samples, 100_000_000, maximum_skew_ns=50_000_000) is None + + +def test_conflicting_values_for_one_receipt_are_invalid(): + samples = (StateSample(10, (10., 20.), (1, 10)), StateSample(20, (11., 21.), (1, 20))) + assert interpolate_state_u8(samples, 10, maximum_skew_ns=50) is None + + +@pytest.mark.parametrize('change', [dict(schema='heartbeat'), dict(stamp_ns=21), + dict(channel_stamps_ns=[0, 20]), dict(position=[1.]), dict(channel_stamps_ns=[1.0, 20])]) +def test_malformed_receipt_contract_is_rejected(change): + payload = dict(schema='can_feedback_v1', name=['a','b'], position=[1., 2.], + stamp_ns=20, channel_stamps_ns=[10, 20]) + with pytest.raises(ValueError): + decode_timed_feedback(json.dumps({**payload, **change})) + + +def test_duplicate_and_out_of_order_cache_do_not_refresh_watchdog(tmp_path): + host, clock = coordinator_fixture(tmp_path) + try: + names, values = host.profile.command.names, host.profile.command.baseline_values + stamp = host.ports.clock_ns() + host.receive_feedback(names, values, stamp, channel_stamps_ns=(stamp,)*len(values)) + first = tuple(host.state_receive_times) + clock.now += .5 + host.receive_feedback(names, values, stamp, channel_stamps_ns=(stamp,)*len(values)) + host.receive_feedback(names, values, stamp-1, channel_stamps_ns=(stamp-1,)*len(values)) + assert tuple(host.state_receive_times) == first + assert len(host.state_history) == 1 + finally: + host.close() + + +@pytest.mark.parametrize('displacement_ns', [-2_000_000_000, 2_000_000_000]) +def test_one_stale_or_future_channel_cannot_be_hidden_by_fresh_channels(tmp_path, displacement_ns): + host, clock = coordinator_fixture(tmp_path) + try: + names, values = host.profile.command.names, host.profile.command.baseline_values + stamps = [host.ports.clock_ns()]*len(values) + stamps[0] += displacement_ns + host.receive_feedback(names, values, max(stamps), channel_stamps_ns=stamps) + assert not host.state_history + assert not host.state_receive_times + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_measured_finalization.py b/src/linkerhand_calibration/test/test_measured_finalization.py index 74ec9b3..21978fc 100644 --- a/src/linkerhand_calibration/test/test_measured_finalization.py +++ b/src/linkerhand_calibration/test/test_measured_finalization.py @@ -25,12 +25,15 @@ def measured_release(tmp_path_factory, request): return profile, source, records, hashes, payload, fit, artifact -def test_nonlinear_full_pair_passes_while_standard_mimic_loss_is_reported(measured_release): +def test_nonlinear_full_pair_and_separate_mimic_approximation_are_reported(measured_release): profile, source, records, hashes, payload, fit, artifact = measured_release assert payload["schema_version"] == 3 acceptance = json.loads(artifact.staged_release.validation_json) assert acceptance["independent_mapping_holdout"] - assert max(v["maximum_deg"] for v in acceptance["urdf_mimic_approximation"].values()) > 3 + assert acceptance["urdf_joint_angle_source"] == "calibration_json" + assert acceptance["urdf_mimic_policy"] == "preserve_source_mimic" + assert not fit.mimic_approximations + assert acceptance["urdf_mimic_approximation"] assert max(v["maximum_deg"] for v in acceptance["full_json_urdf_holdout"].values()) < 1 reader = load_unified_mapper(artifact.path.parent/"release_manifest.json") assert np.max(np.abs(reader.map_positions(profile.command.baseline_values))) < 1e-10 @@ -52,6 +55,8 @@ def test_nonlinear_full_pair_passes_while_standard_mimic_loss_is_reported(measur sdk.publish_position(values) assert writes[-1] == tuple(values) angles = dict(zip(reader.urdf_joint_names, reader.map_positions(values))) + assert any(joint.mimic_joint for joint in reader.urdf.joints.values()) + assert reader.urdf.resolve_angles(angles, independent_mimic_angles=True) == angles for name, row in payload["joints"].items(): assert row["sdk_channel"] == channel_for_joint(profile, name) assert angles[name] == pytest.approx(row["angle_rad"][int(values[row["sdk_channel"]])], abs=1e-12) diff --git a/src/linkerhand_calibration/test/test_measured_transfer_finalization.py b/src/linkerhand_calibration/test/test_measured_transfer_finalization.py index d7f929e..9742957 100644 --- a/src/linkerhand_calibration/test/test_measured_transfer_finalization.py +++ b/src/linkerhand_calibration/test/test_measured_transfer_finalization.py @@ -16,9 +16,39 @@ from linkerhand_calibration.runtime.artifacts.finalization import finalize_profi from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper -@pytest.mark.parametrize("reordered", [False, True], ids=["o6", "reordered_virtual_o6"]) -def test_o6_records_fit_expand_publish_and_read_back_without_recipient_observations(tmp_path, reordered): +@pytest.mark.parametrize("reordered,separate_mapping", [(False, False), (True, False), (False, True)], + ids=["o6", "reordered_virtual_o6", "separate_command_mapping"]) +def test_o6_records_fit_expand_publish_and_read_back_without_recipient_observations(tmp_path, monkeypatch, reordered, separate_mapping): profile, source, records, offsets = measured_capture(tmp_path, layout="o6_right_8", reordered=reordered) + if separate_mapping: + # This is synthetic evidence: exercise distinct motion and command + # partitions without claiming an additional physical recording. + from dataclasses import replace + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate")) + records = [dict(r, cycle={0: 4, 3: 5}[r["cycle"]]) if r.get("sample_phase") == "steady" else r + for r in records if r.get("sample_phase") != "steady" or r["cycle"] in {0, 3}] + from linkerhand_calibration.core.fitting.command_sampling import build_sampling_plan + plans = {task.key: build_sampling_plan(profile, task, records) for task in profile.motion.tasks} + selected = [] + for row in records: + if row.get("sample_phase") != "steady": + selected.append(row) + continue + plan = plans[row["task_name"]] + if row["cycle"] == 4 and row["steady_target"] not in plan["training_nodes"]: + continue + selected.append(dict(row, steady_training_nodes=plan["training_nodes"], sampling_plan_sha256=plan["plan_sha256"])) + records = [*selected, *plans.values()] + from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher + from test_final_image_holdout import independent_pixels + original = ArtifactPublisher.prepare + def prepare(publisher, **kwargs): + validator = kwargs["validate"] + kwargs["validate"] = replace(validator, require_image_holdout=True, + image_observations=independent_pixels(validator.observations), + command_image_observations=independent_pixels(validator.command_observations)) + return original(publisher, **kwargs) + monkeypatch.setattr(ArtifactPublisher, "prepare", prepare) source_bytes = source.read_bytes() transfers = dict(profile.measurement.transferred_motion_sources) assert len(transfers) == 6 @@ -37,14 +67,34 @@ def test_o6_records_fit_expand_publish_and_read_back_without_recipient_observati assert len(moving) == 11 and set(payload["joints"]) == moving assert set(fit.curves) == set(fit.joint_zero_references) == set(fit.command_holdout_metrics) == observed assert set(fit.command_mappings) == set(fit.output_mappings) == moving + declared = profile.artifacts.directional_command_joints + assert set(payload["directional_command_joints"]) == declared + for name, evidence in fit.command_applicability.items(): + assert (evidence.get("command_mapping") == "directional") == (name in declared) report = json.loads((artifact.path.parent/"calibration_report.json").read_text()) acceptance = json.loads(artifact.staged_release.validation_json) + if separate_mapping: + assert acceptance["final_file_image_holdout_verified"] is True + assert set(acceptance["final_file_image_holdout"]) == {"feedback", "command"} + assert (artifact.path.parent/"command_sampling_plans.json").is_file() + assert acceptance["command_holdout_cycle"] == (5 if separate_mapping else 3) assert acceptance["acceptance"] == "measured_and_transferred_json_urdf_v3" assert set(acceptance["independent_mapping_holdout"]) == observed assert acceptance["transferred_motion_sources"] == transfers observed_roles = {profile.measurement.measurements[name].child_role for name in observed} assert set(acceptance["full_json_urdf_holdout"]) == observed_roles - assert max(row["maximum_deg"] for row in acceptance["urdf_mimic_approximation"].values()) > 3 + assert acceptance["urdf_joint_angle_source"] == "calibration_json" + assert acceptance["urdf_mimic_policy"] == "preserve_source_mimic" + assert not fit.mimic_approximations + assert acceptance["urdf_mimic_approximation"] + original_model, exported_model = UrdfKinematicModel(source), UrdfKinematicModel(artifact.path) + assert {name: j.mimic_joint for name, j in exported_model.joints.items()} == { + name: j.mimic_joint for name, j in original_model.joints.items()} + for name, original in original_model.joints.items(): + if original.mimic_joint: + actual = exported_model.joints[name] + assert (actual.mimic_multiplier, actual.mimic_offset) == ( + original.mimic_multiplier, original.mimic_offset) for target, donor in transfers.items(): assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] assert payload["joints"][target]["sdk_channel"] == channel_for_joint(profile, target) diff --git a/src/linkerhand_calibration/test/test_motion_branch_evidence.py b/src/linkerhand_calibration/test/test_motion_branch_evidence.py new file mode 100644 index 0000000..27980b3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_branch_evidence.py @@ -0,0 +1,257 @@ +"""Physical motion evidence can authorize a branch only before zero freeze.""" + +from dataclasses import FrozenInstanceError, replace +import json +import math +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( + MotionEvidenceFrame, MotionEvidenceParameters, MotionRelation, RoleCandidates, + resolve_motion_branches, select_motion_frame, +) +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + + +RELATIONS = (MotionRelation("joint_a", "base", "parent"), + MotionRelation("joint_b", "parent", "child")) + + +def pose(rotation, translation, error=0.4): + return SquareTagPose(tuple(rotation.as_quat()), tuple(translation), error) + + +def motion_frames(*, span_deg=80, samples=81, bad_alternative=True, radius=0.05): + """Two independently articulated links, each with an image-favored false root.""" + frames = [] + for i, theta in enumerate(np.linspace(0, math.radians(span_deg), samples)): + base = pose(Rotation.identity(), (0, 0, 1), 0.1) + parent_r = Rotation.from_rotvec([0, 0, theta]) + parent_t = np.array([0, 0, 1]) + parent_r.apply([radius, 0, 0]) + child_r = parent_r * Rotation.from_rotvec([theta * 0.4, 0, 0]) + child_t = parent_t + parent_r.apply([0.03, 0, 0]) + child_r.apply([0, radius, 0]) + parent = pose(parent_r, parent_t) + child = pose(child_r, child_t) + # Change orientation, not measured image translation. The alternate + # cannot describe the same hinge axis and pivot over a sufficient arc. + wrong_r = Rotation.from_euler("y", 35, degrees=True) + parent_alt = pose(wrong_r * parent_r, parent_t, 0.1) + child_alt = pose(wrong_r * child_r, child_t, 0.1) + if not bad_alternative: + # An unknown constant Tag mounting rotation is unobservable from + # this motion: both families are physically admissible. + parent_alt = pose(parent_r * Rotation.from_euler("z", 40, degrees=True), parent_t, 0.1) + child_alt = pose(child_r * Rotation.from_euler("z", 40, degrees=True), child_t, 0.1) + frames.append(MotionEvidenceFrame(1_000_000_000 + i * 50_000_000, ( + RoleCandidates("base", (base,), frozen=True), + RoleCandidates("parent", (parent_alt, parent)), + RoleCandidates("child", (child_alt, child)), + ))) + return frames + + +def test_joint_parent_child_geometry_selects_actual_candidates_not_image_minimum(): + frames = motion_frames() + result = resolve_motion_branches(frames, RELATIONS) + assert result.resolved, result + assert len(result.constraints) == 2 + assert [v.pose for v in result.selections] == [v.candidates[-1] for v in frames[-1].roles] + assert sum(h.accepted for h in result.hypotheses) == 1 + assert all(model.source_stamps_ns for model in result.constraints) + with pytest.raises(FrozenInstanceError): + result.constraints[0].maximum_pivot_error_m = 1 + + +def test_postfreeze_selection_uses_same_immutable_model_and_original_current_candidates(): + frames = motion_frames() + result = resolve_motion_branches(frames[::2], RELATIONS) + assert result.resolved + models_before = result.constraints + for frame in frames[1::2]: + selected = select_motion_frame(result.constraints, frame.roles) + assert selected.resolved, selected + assert all(item.pose is frame.roles[i].candidates[-1] for i, item in enumerate(selected.selections)) + assert result.constraints == models_before + + +def test_constant_mount_ambiguity_is_not_resolved_by_lowest_error_or_smoothness(): + result = resolve_motion_branches(motion_frames(bad_alternative=False), RELATIONS) + assert not result.resolved + assert result.reason == "motion_geometry_ambiguous" + assert result.constraints == result.selections == () + + +@pytest.mark.parametrize("span", [0, 3, 10]) +def test_insufficient_rotation_cannot_freeze_even_with_many_frames(span): + result = resolve_motion_branches(motion_frames(span_deg=span, samples=300), RELATIONS) + assert not result.resolved + assert any(q.reason == "insufficient_rotation_evidence" for h in result.hypotheses for q in h.relations) + + +def test_stationary_endpoints_do_not_inflate_motion_evidence(): + samples = motion_frames(samples=5) + repeated = [replace(frame, stamp_ns=1_000_000_000 + i * 10_000_000) + for i, frame in enumerate(frame for frame in samples for _ in range(100))] + result = resolve_motion_branches(repeated, RELATIONS) + assert not result.resolved + assert all(q.motion_bins <= 5 for h in result.hypotheses for q in h.relations) + + +def test_early_late_validation_rejects_a_mid_approach_mount_shift(): + frames = motion_frames() + changed = [] + for index, frame in enumerate(frames): + roles = list(frame.roles) + if index > len(frames) // 2: + roles[1] = replace(roles[1], candidates=tuple(replace(p, + translation_xyz_m=tuple(np.asarray(p.translation_xyz_m) + [0, 0, 0.002])) + for p in roles[1].candidates)) + changed.append(replace(frame, roles=tuple(roles))) + result = resolve_motion_branches(changed, RELATIONS[:1]) + assert not result.resolved + quality = result.hypotheses[1].relations[0] + assert quality.pivot_rms_m < 0.0015 # A pooled fit alone would hide this shift. + assert quality.reason == "motion_split_validation_failed" + + +def test_rotation_without_resolved_position_arc_is_insufficient(): + result = resolve_motion_branches(motion_frames(radius=0), RELATIONS[:1]) + assert not result.resolved + assert all(h.relations[0].reason == "insufficient_pivot_translation" for h in result.hypotheses) + + +def test_image_identity_and_changed_frozen_contract_are_rejected(): + frames = motion_frames() + result = resolve_motion_branches([frames[0], *frames], RELATIONS) + assert result.reason == "motion_evidence_image_identity_invalid" + roles = frames[20].roles + frames[20] = replace(frames[20], roles=(replace(roles[0], frozen=False), roles[1], roles[2])) + assert resolve_motion_branches(frames, RELATIONS).reason == "motion_candidate_family_interrupted" + + +@pytest.mark.parametrize("invalid", ["reprojection", "tilt", "nan"]) +def test_invalid_alternative_does_not_discard_the_other_legal_candidate(invalid): + frames = motion_frames() + first = frames[0] + roles = list(first.roles) + target = roles[1].candidates[0] + if invalid == "reprojection": + target = replace(target, reprojection_error_px=1.50001) + elif invalid == "tilt": + target = replace(target, quaternion_xyzw=tuple(Rotation.from_euler("x", 76, degrees=True).as_quat())) + else: + target = replace(target, translation_xyz_m=(float("nan"), 0, 1)) + roles[1] = replace(roles[1], candidates=(target, roles[1].candidates[-1])) + frames[0] = replace(first, roles=tuple(roles)) + result = resolve_motion_branches(frames, RELATIONS) + assert result.resolved + selected = select_motion_frame(result.constraints, frames[0].roles) + assert selected.resolved and selected.selections[1].pose is frames[0].roles[1].candidates[-1] + bad_only = (*frames[0].roles[:1], replace(roles[1], candidates=(target,)), roles[2]) + assert not select_motion_frame(result.constraints, bad_only).resolved + + +def test_temporarily_missing_root_is_not_copied_to_another_family(): + frames = motion_frames() + roles = frames[20].roles + frames[20] = replace(frames[20], roles=(roles[0], + replace(roles[1], candidates=roles[1].candidates[:1]), roles[2])) + result = resolve_motion_branches(frames, RELATIONS) + assert result.resolved + assert all(frames[20].stamp_ns not in model.source_stamps_ns for model in result.constraints) + + +def test_existing_frozen_candidate_is_never_replaced_by_better_geometry(): + frames = motion_frames() + fixed_wrong = [replace(frame, roles=(frame.roles[0], + RoleCandidates("parent", (frame.roles[1].candidates[0],), frozen=True), frame.roles[2])) for frame in frames] + result = resolve_motion_branches(fixed_wrong, RELATIONS) + assert not result.resolved + assert result.constraints == result.selections == () + + +def test_frozen_role_cannot_offer_two_roots_to_training(): + frames = motion_frames() + frame = frames[0] + frames[0] = replace(frame, roles=(frame.roles[0], replace(frame.roles[1], frozen=True), frame.roles[2])) + assert resolve_motion_branches(frames, RELATIONS).reason == "motion_candidates_missing_or_invalid" + + +def test_postfreeze_tag_slide_is_rejected_without_model_update(): + frames = motion_frames() + result = resolve_motion_branches(frames, RELATIONS) + frame = frames[40] + roles = list(frame.roles) + roles[2] = replace(roles[2], candidates=tuple(replace(p, + translation_xyz_m=tuple(np.asarray(p.translation_xyz_m) + [0, 0, 0.01])) for p in roles[2].candidates)) + selected = select_motion_frame(result.constraints, tuple(roles)) + assert not selected.resolved + assert selected.reason == "motion_geometry_inconsistent" + + +def test_postfreeze_two_physical_choices_remain_ambiguous(): + frames = motion_frames() + result = resolve_motion_branches(frames, RELATIONS[:1]) + frame = frames[40] + # Two distinct points along the same previously fitted hinge both explain + # their candidate poses. Geometry alone must not pick the smaller error. + role = RoleCandidates("parent", (frame.roles[1].candidates[-1], frames[50].roles[1].candidates[-1])) + selected = select_motion_frame(result.constraints, (frame.roles[0], role)) + assert not selected.resolved and selected.reason == "motion_geometry_ambiguous" + + +def real_frames(direction, field="frames"): + source = Path(__file__).with_name("fixtures") / "o6_thumb_20260914_175815_motion_evidence.json" + fixture = json.loads(source.read_text()) + assert fixture["scope"] == "single_round_counterfactual_branch_regression_not_accuracy_certificate" + assert fixture["source_raw_sha256"] == "8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d" + return [MotionEvidenceFrame(frame["stamp_ns"], tuple(RoleCandidates(role["role"], tuple( + SquareTagPose(tuple(p["quaternion_xyzw"]), tuple(p["translation_xyz_m"]), p["reprojection_error_px"]) + for p in role["candidates"]), role["frozen"]) for role in frame["roles"])) + for frame in fixture[field] if frame["direction"] == direction] + + +def test_real_top_yaw_retry_has_unique_motion_evidence(): + frames = real_frames("decreasing", "yaw_retry_frames") + result = resolve_motion_branches(frames, (MotionRelation("yaw", "top_base", "thumb_yaw"),)) + assert result.resolved + assert len(frames) == 210 + assert sum(h.accepted for h in result.hypotheses) == 1 + quality = next(h.relations[0] for h in result.hypotheses if h.accepted) + assert quality.motion_rad > math.radians(65) + assert quality.pivot_rms_m < 0.0015 + + +@pytest.mark.parametrize("direction", ["decreasing", "increasing"]) +def test_real_capture_pitch_has_evidence_but_full_ip_chain_must_remain_unresolved(direction): + frames = real_frames(direction) + pitch = MotionRelation("pitch", "front_base", "thumb_pitch") + ip = MotionRelation("ip", "thumb_pitch", "thumb_ip") + # This is explicitly counterfactual replay of sweep candidates. Production + # must never use it to replace this real session's already frozen zero. + result = resolve_motion_branches(frames, (pitch,)) + assert result.resolved + assert sum(h.accepted for h in result.hypotheses) == 1 + assert all(h.relations[0].pivot_rms_m < 0.0015 for h in result.hypotheses if h.accepted) + joint = resolve_motion_branches(frames, (pitch, ip)) + assert not joint.resolved and joint.reason == "motion_geometry_unresolved" + assert all(h.relations[1].reason == "motion_geometry_residual" for h in joint.hypotheses) + assert joint.constraints == joint.selections == () + + +def test_real_reverse_direction_uses_first_direction_model_without_refitting(): + pitch = MotionRelation("pitch", "front_base", "thumb_pitch") + result = resolve_motion_branches(real_frames("decreasing"), (pitch,)) + assert result.resolved + accepted = [select_motion_frame(result.constraints, frame.roles).resolved for frame in real_frames("increasing")] + assert all(accepted) + + +@pytest.mark.parametrize("field,value", [("maximum_pivot_rms_m", float("nan")), + ("angular_bin_rad", 0), ("minimum_motion_bins", 11), ("minimum_motion_bins", 12.0)]) +def test_invalid_parameters_are_rejected(field, value): + with pytest.raises(ValueError): + MotionEvidenceParameters(**{field: value}) diff --git a/src/linkerhand_calibration/test/test_motion_capture_tracking.py b/src/linkerhand_calibration/test/test_motion_capture_tracking.py new file mode 100644 index 0000000..a15d567 --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_capture_tracking.py @@ -0,0 +1,221 @@ +"""Capture uses frozen motion evidence even while recording is not allowed. + +The fixture injects explicit IPPE candidates to isolate the real tracker, +constraint selector and capture boundary. It fits its immutable constraints +from the independently articulated synthetic trajectory in the numeric tests; +these checks do not claim image or hardware accuracy. +""" + +from concurrent.futures import ThreadPoolExecutor +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( + MotionRelation, resolve_motion_branches, +) +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime import capture as capture_module +from linkerhand_calibration.runtime.branch_initialization import MotionBranchModel +from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.reference_lock import ReferenceLock + +from test_motion_branch_evidence import motion_frames + + +STEP = 40_000_000 +START = 10_000_000_000 +ROLE_NAMES = {"base": "front_base", "parent": "thumb_pitch", "child": "thumb_ip"} +MOVING = ("thumb_pitch", "thumb_ip") +IDS = {"thumb_pitch": "a" * 64, "thumb_ip": "b" * 64} +JOINTS = ("rh_thumb_cmc_pitch", "rh_thumb_ip") +TASK = "thumb_pitch_ip_front" + + +@pytest.fixture +def rig(monkeypatch): + class Rig: + def __init__(self): + self.profile = load_bundled_hand_profile("o6_right_8") + self.frames = [replace(frame, roles=tuple( + replace(item, role=ROLE_NAMES[item.role]) for item in frame.roles)) + for frame in motion_frames()] + relations = (MotionRelation(JOINTS[0], "front_base", "thumb_pitch"), + MotionRelation(JOINTS[1], "thumb_pitch", "thumb_ip")) + resolution = resolve_motion_branches(self.frames, relations) + assert resolution.resolved + self.model = MotionBranchModel(TASK, "front", resolution.constraints, tuple(IDS.items())) + # Fixed reference image verification is independently covered by + # test_fixed_reference_pose. Here base has one current valid root. + lock = ReferenceLock({view.name: next(tag.tag_id for tag in view.tags if tag.fixed_reference) + for view in self.profile.vision.views}) + self.capture = ObservationCapture(self.profile, reference_lock=lock, + extrinsics=SimpleNamespace(transform=lambda view: np.eye(4))) + self.tracker = self.capture.trackers["front"] + self.current = {item.role: item.candidates for item in self.frames[20].roles} + self.role_by_marker = {index: role for index, role in enumerate(ROLE_NAMES.values(), start=1)} + self.corners = {role: np.array([[index, 0.], [index + .1, 0.], + [index + .1, .1], [index, .1]]) for index, role in self.role_by_marker.items()} + self.motion = MotionCommand("joint_zero", tuple(self.profile.command.baseline_values), 40, + task_key=TASK, command_index=0, cycle=0, zero_joints=JOINTS) + + def solve(self, corners, **kwargs): + return list(self.current[self.role_by_marker[int(corners[0][0])]]) + + def frame(self, counter, *, index=20, omit=(), slide=False): + self.current = {item.role: item.candidates for item in self.frames[index].roles} + if slide: + self.current["thumb_ip"] = tuple(replace(pose, + translation_xyz_m=tuple(np.asarray(pose.translation_xyz_m) + [0, 0, .01])) + for pose in self.current["thumb_ip"]) + values = tuple(map(float, self.profile.command.baseline_values)) + return CaptureFrame("front", START + counter * STEP, np.eye(3), + {role: points for role, points in self.corners.items() if role not in omit}, values, values) + + def establish(self): + observation = self.frame(10) + rows, movement = self.capture.consume(observation, self.motion, allow_recording=False) + assert movement is None and not any(row["kind"] == "joint_zero_sample" for row in rows) + revisions = {role: self.tracker.motion_snapshot(role, observation.stamp_ns).revision for role in MOVING} + assert self.tracker.freeze_roles(revisions, require_motion_evidence=True) + return revisions + + instance = Rig() + monkeypatch.setattr(tracking, "solve_square_tag_ippe", instance.solve) + for counter in range(5): + instance.capture.consume(instance.frame(counter)) + # Both incorrect candidates have decisively smaller reprojection errors. + # The legacy tracker has actually confirmed those candidates during preview. + for role in MOVING: + assert instance.tracker._branches.states[role].pose == instance.current[role][0] + instance.capture.install_motion_model(instance.model) + return instance + + +def _joint_rows(rows): + return [row for row in rows if row["kind"] in {"joint_zero_sample", "joint_sample", "secondary_joint_sample"}] + + +def test_unstable_zero_and_recording_transitions_keep_frozen_geometry_and_identity(rig, monkeypatch): + original_update = rig.tracker._branches.update + + def only_fixed_independent_update(role, *args, **kwargs): + assert role not in MOVING, "an installed motion model must not fall back to image ranking" + return original_update(role, *args, **kwargs) + + monkeypatch.setattr(rig.tracker._branches, "update", only_fixed_independent_update) + revisions = rig.establish() + for counter, allowed in enumerate((False, False, True, False, False, True), start=11): + observation = rig.frame(counter, index=counter + 10) + rows, movement = rig.capture.consume(observation, rig.motion, allow_recording=allowed) + assert movement is None + assert len(_joint_rows(rows)) == (2 if allowed else 0) + for role in MOVING: + state = rig.tracker._branches.states[role] + diagnostics = rig.tracker.observation_snapshot(role, observation.stamp_ns)[1] + assert state.pose == rig.current[role][-1] + assert state.pose.reprojection_error_px > rig.current[role][0].reprojection_error_px + .03 + assert state.revision == revisions[role] and state.frozen + assert state.motion_evidence_id == IDS[role] + assert diagnostics["pose_constraint"] == "motion_geometry" + assert diagnostics["branch_status"] == "tracking" + for row in _joint_rows(rows): + for item in row["pnp_observation_evidence"].values(): + role = item["tag_role"] + if role in MOVING: + assert item["candidate_diagnostics"]["motion_evidence_id"] == IDS[role] + assert item["candidate_diagnostics"]["branch_revision"] == revisions[role] + assert item["selected_pose"]["quaternion_xyzw"] == list(rig.current[role][-1].quaternion_xyzw) + assert rig.capture._motion_models[(TASK, "front")] == rig.model + + +def test_capture_serializes_detached_commit_evidence_after_another_candidate_producer_advances_tracker(rig, monkeypatch): + rig.establish() + observation = rig.frame(11, index=21) + original_snapshot = capture_module.snapshot_pose_evidence + advanced = False + + def snapshot_after_next_candidate_frame(tags, *args, **kwargs): + nonlocal advanced + if "thumb_pitch" in tags and not advanced: + advanced = True + + def advance_candidates(): + for role in MOVING: + rig.tracker.estimate(role, observation.corners[role], tag_size_m=.016, + camera_matrix=observation.camera_matrix, stamp_ns=observation.stamp_ns + STEP, + motion_managed=True) + + # This specifically exercises the tracker/evidence race. Full + # capture-generation invalidation has separate coordinator tests. + with ThreadPoolExecutor(max_workers=1) as executor: + executor.submit(advance_candidates).result(timeout=2) + return original_snapshot(tags, *args, **kwargs) + + monkeypatch.setattr(capture_module, "snapshot_pose_evidence", snapshot_after_next_candidate_frame) + rows, movement = rig.capture.consume(observation, rig.motion) + assert advanced and movement is None + samples = _joint_rows(rows) + assert len(samples) == 2 + for row in samples: + assert row["image_stamp_ns"] == observation.stamp_ns + for item in row["pnp_observation_evidence"].values(): + role = item["tag_role"] + if role in MOVING: + assert rig.tracker.observation_snapshot(role, observation.stamp_ns) is None + diagnostics = item["candidate_diagnostics"] + assert diagnostics["observation_stamp_ns"] == observation.stamp_ns + assert diagnostics["branch_status"] == "tracking" + assert diagnostics["motion_selection_committed"] is True + assert item["selected_pose"]["quaternion_xyzw"] == list(rig.current[role][-1].quaternion_xyzw) + + +def test_one_inconsistent_geometry_frame_is_discarded_without_refreezing_or_latching_conflict(rig): + revisions = rig.establish() + previous = {role: rig.tracker._branches.states[role].pose for role in MOVING} + rows, movement = rig.capture.consume(rig.frame(11, index=21, slide=True), rig.motion) + assert movement is None and not _joint_rows(rows) + events = [row for row in rows if row["kind"] == "pnp_branch_event"] + assert {row["tag_role"] for row in events} == set(MOVING) + assert all(row["reason"] == "pose_branch_motion_geometry_inconsistent" for row in events) + for role in MOVING: + state = rig.tracker._branches.states[role] + assert state.pose == previous[role] + assert state.revision == revisions[role] and state.motion_evidence_id == IDS[role] + assert state.frozen and not state.conflict + rows, movement = rig.capture.consume(rig.frame(12, index=22), rig.motion) + assert movement is None and len(_joint_rows(rows)) == 2 + assert rig.capture._motion_models[(TASK, "front")] == rig.model + assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) + + +def test_missing_articulated_tag_does_not_promote_the_other_moving_candidate(rig): + revisions = rig.establish() + observation = rig.frame(11, omit=("thumb_ip",)) + rows, movement = rig.capture.consume(observation, rig.motion) + assert movement is None and not _joint_rows(rows) + assert rig.capture.last_missing["front"] == (2,) + assert not rig.tracker._branches.states["thumb_pitch"].conflict + rows, movement = rig.capture.consume(rig.frame(12, index=21), rig.motion) + assert movement is None and len(_joint_rows(rows)) == 2 + assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) + + +def test_two_current_geometric_solutions_are_not_resolved_by_image_error(rig): + revisions = rig.establish() + observation = rig.frame(11, index=21) + alternative = {item.role: item.candidates[-1] for item in rig.frames[25].roles} + for role in MOVING: + # Both pairs lie on the same frozen articulated motion, so neither + # geometric consistency nor lower image error determines physical truth. + rig.current[role] = (rig.current[role][-1], replace(alternative[role], reprojection_error_px=.05)) + rows, movement = rig.capture.consume(observation, rig.motion) + assert movement is None and not _joint_rows(rows) + events = [row for row in rows if row["kind"] == "pnp_branch_event"] + assert {row["tag_role"] for row in events} == set(MOVING) + assert all(row["reason"] == "pose_branch_motion_geometry_ambiguous" for row in events) + assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) diff --git a/src/linkerhand_calibration/test/test_motion_capture_transitions.py b/src/linkerhand_calibration/test/test_motion_capture_transitions.py new file mode 100644 index 0000000..b7c284d --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_capture_transitions.py @@ -0,0 +1,45 @@ +"""Model identity belongs to a physical Tag, including between task streams.""" + +from dataclasses import replace + +import pytest + +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from test_motion_capture_tracking import IDS, MOVING, rig + + +@pytest.mark.parametrize("transition", ["idle", "baseline", "return", "reference_return", + "mapping_probe", "clearance", "different_task"]) +def test_installed_tag_never_returns_to_ippe_during_nonrecording_transitions(rig, monkeypatch, transition): + revisions = rig.establish() + update = rig.tracker._branches.update + + def fixed_tags_only(role, *args, **kwargs): + assert role not in MOVING, "a model-authorized Tag must never return to independent IPPE" + return update(role, *args, **kwargs) + + monkeypatch.setattr(rig.tracker._branches, "update", fixed_tags_only) + motion = None + if transition != "idle": + task = rig.profile.motion.tasks[0] if transition == "different_task" else None + motion = MotionCommand("prepare" if task else transition, + rig.profile.command.baseline_values, 40., task_key=None if task is None else task.key) + for counter in range(11, 15): + frame = rig.frame(counter, index=counter + 10) + rows, movement = rig.capture.consume(frame, motion) + assert movement is None + assert not any(row["kind"] in {"joint_sample", "joint_zero_sample", "pnp_candidate_frame"} for row in rows) + for role in MOVING: + state = rig.tracker._branches.states[role] + assert state.pose == rig.current[role][-1] + assert state.revision == revisions[role] and state.motion_evidence_id == IDS[role] + assert state.frozen and not state.conflict + + +def test_a_later_task_still_keeps_prior_model_without_recording_prior_joint(rig): + rig.establish() + other = rig.profile.motion.tasks[0] + motion = replace(rig.motion, phase="sweep", task_key=other.key, direction="decreasing", zero_joints=()) + rows, _ = rig.capture.consume(rig.frame(11, index=21), motion) + assert not any(row.get("joint") in {"rh_thumb_cmc_pitch", "rh_thumb_ip"} for row in rows) + assert all(rig.tracker._branches.states[role].pose == rig.current[role][-1] for role in MOVING) diff --git a/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py b/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py new file mode 100644 index 0000000..a790691 --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py @@ -0,0 +1,273 @@ +"""Geometric branch authorization must remain tied to one image and reference. + +These tests exercise the metadata contract with synthetic geometric decisions; +physical motion-model selection has its own numeric tests. +""" + +from dataclasses import FrozenInstanceError, replace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.core.geometry.tag_pose.tracking import ( + MotionPoseSelection, SquareTagPoseTracker, +) +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + + +START = 1_000_000_000 +STEP = 33_000_000 +EVIDENCE = "a" * 64 + + +def pose(angle=0., error=.05, translation=(0., 0., .5)): + return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), + translation, error) + + +@pytest.fixture +def source(monkeypatch): + class Source: + candidates = (pose(0., .20), pose(60., .05)) + + def observe(self, tracker, stamp, role="moving", managed=True): + result = tracker.estimate(role, ((0, 0), (1, 0), (1, 1), (0, 1)), + tag_size_m=.016, camera_matrix=np.eye(3), stamp_ns=stamp, + motion_managed=managed) + return result, tracker.motion_snapshot(role, stamp) + + instance = Source() + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: list(instance.candidates)) + return instance + + +def selection(source, tracker, stamp, *, role="moving", candidate=0, evidence=EVIDENCE): + result, snapshot = source.observe(tracker, stamp, role) + assert result[0] is None + return MotionPoseSelection(snapshot, source.candidates[candidate], evidence) + + +def establish(source, tracker, stamp=START, *, role="moving"): + choice = selection(source, tracker, stamp, role=role) + result = tracker.commit_motion_frame((choice,), establish=True) + assert result.accepted + return tracker.motion_snapshot(role, stamp) + + +def test_motion_managed_images_never_confirm_an_image_winner_without_geometry(source): + tracker = SquareTagPoseTracker() + for i in range(20): + outcome, snapshot = source.observe(tracker, START + i * STEP) + assert outcome == (None, "pose_motion_evidence_required") + assert snapshot.revision == 0 and snapshot.evidence_id is None + assert not tracker.freeze_roles({"moving": 0}, require_motion_evidence=True) + + +def test_establishment_authorizes_current_candidate_without_increasing_point03_tolerance(source): + tracker = SquareTagPoseTracker() + choice = selection(source, tracker, START) + assert not tracker.commit_motion_frame((choice,)).accepted + # The failed authorization request consumed that decision's frame. + choice = selection(source, tracker, START + STEP) + result = tracker.commit_motion_frame((choice,), establish=True) + assert result.accepted and result.selections[0].pose == source.candidates[0] + detail = result.observation_snapshots[0].diagnostics + assert detail["motion_evidence_verified"] is True + assert detail["motion_evidence_id"] == EVIDENCE + assert detail["branch_revision"] == 1 + assert detail["branch_status"] == "tracking" + assert tracker.reprojection_tie_px == .03 + assert tracker.freeze_roles({"moving": 1}, require_motion_evidence=True) + + +def test_geometry_correction_is_explicit_and_only_before_freeze(source): + tracker = SquareTagPoseTracker() + for i in range(5): + result, _ = source.observe(tracker, START + i * STEP, managed=False) + assert result[0] == source.candidates[1] + old_revision = tracker.motion_snapshot("moving", START + 4 * STEP).revision + choice = selection(source, tracker, START + 5 * STEP) + result = tracker.commit_motion_frame((choice,), establish=True) + assert result.accepted + assert result.observation_snapshots[0].diagnostics["branch_revision"] == old_revision + 1 + assert result.observation_snapshots[0].corrections == 1 + assert not tracker.freeze_roles({"moving": old_revision}, require_motion_evidence=True) + + +def test_existing_image_only_frozen_reference_cannot_be_reauthorized(source): + tracker = SquareTagPoseTracker() + for i in range(5): + source.observe(tracker, START + i * STEP, managed=False) + assert not tracker.freeze_roles({"moving": 1}, require_motion_evidence=True) + assert tracker.freeze_roles({"moving": 1}) + choice = selection(source, tracker, START + 5 * STEP) + assert not tracker.commit_motion_frame((choice,), establish=True).accepted + assert tracker.motion_snapshot("moving", START + 5 * STEP).evidence_id is None + + +def test_frozen_geometry_keeps_current_candidate_when_image_winner_changes(source): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + for i in range(1, 190): + source.candidates = (pose(.001 * i, .20), pose(60., .05)) + result = tracker.commit_motion_frame((selection(source, tracker, START + i * STEP),)) + assert result.accepted + detail = result.observation_snapshots[0].diagnostics + assert detail["branch_revision"] == initial.revision + assert detail["branch_frozen"] is True + assert result.observation_snapshots[0].corrections == 0 + + +@pytest.mark.parametrize("candidate", [pose(0., 1.50001), pose(76., .05)]) +def test_geometry_cannot_bypass_reprojection_or_tilt_gate(source, candidate): + tracker = SquareTagPoseTracker() + source.candidates = (candidate,) + choice = selection(source, tracker, START) + assert choice.snapshot.candidates == () + result = tracker.commit_motion_frame((choice,), establish=True) + assert not result.accepted and result.reason == "pose_motion_selection_invalid" + + +def test_geometry_cannot_insert_an_unobserved_pose(source): + tracker = SquareTagPoseTracker() + choice = selection(source, tracker, START) + result = tracker.commit_motion_frame((replace(choice, pose=pose(1.)),), establish=True) + assert not result.accepted and result.reason == "pose_motion_selection_invalid" + + +def test_frozen_role_cannot_change_evidence_id_or_jump_branch(source): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + changed = selection(source, tracker, START + STEP, evidence="b" * 64) + assert tracker.commit_motion_frame((changed,), establish=True).reason == "pose_motion_evidence_changed" + discontinuous = selection(source, tracker, START + 2 * STEP, candidate=1) + assert tracker.commit_motion_frame((discontinuous,)).reason == "pose_motion_branch_discontinuous" + snapshot = tracker.motion_snapshot("moving", START + 2 * STEP) + assert snapshot.evidence_id == EVIDENCE and snapshot.revision == initial.revision + result = tracker.commit_motion_frame((selection(source, tracker, START + 3 * STEP),)) + assert result.accepted + + +@pytest.mark.parametrize("interruption", ["reset", "new_image", "freeze"]) +def test_late_geometry_result_cannot_commit_after_reference_or_image_changes(source, interruption): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + choice = selection(source, tracker, START + STEP) + if interruption == "reset": + tracker.reset() + elif interruption == "new_image": + source.observe(tracker, START + 2 * STEP) + else: + # Freeze needs a committed frame. This simulates a different authorized + # consumer committing and then freezing while an old solve was running. + assert tracker.commit_motion_frame((choice,)).accepted + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + result = tracker.commit_motion_frame((choice,)) + assert not result.accepted and result.reason == "pose_observation_superseded" + + +def test_partial_geometry_batch_never_establishes_only_one_role(source): + tracker = SquareTagPoseTracker() + first = selection(source, tracker, START, role="parent") + second = selection(source, tracker, START, role="child") + invalid = replace(second, pose=pose(2.)) + assert not tracker.commit_motion_frame((first, invalid), establish=True).accepted + assert tracker.motion_snapshot("parent", START).evidence_id is None + assert tracker.motion_snapshot("child", START).evidence_id is None + assert tracker.commit_motion_frame((first, second), establish=True).accepted + + +def test_batch_checks_all_versions_before_any_mutation(source): + first, second = SquareTagPoseTracker(), SquareTagPoseTracker() + choices = (selection(source, first, START, role="parent"), + selection(source, second, START, role="child")) + second.reset() + assert not first.commit_motion_frame(choices, establish=True).accepted + assert first.motion_snapshot("parent", START).evidence_id is None + + +def test_real_time_gap_retains_frozen_recovery_and_unique_image_requirement(source): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + source.candidates = (pose(2., .20), pose(60., .05)) + stamp = START + 6_000_000_000 + result = tracker.commit_motion_frame((selection(source, tracker, stamp),)) + assert result.reason == "pose_branch_recovery_required" + source.candidates = (pose(.2, .20), pose(60., .05)) + for i in range(1, 6): + choice = selection(source, tracker, stamp + i * STEP) + result = tracker.commit_motion_frame((choice,)) + assert result.accepted is (i == 5) + repeated = tracker.commit_motion_frame((choice,)) + assert not repeated.accepted and repeated.reason == "pose_observation_superseded" + assert tracker.motion_snapshot("moving", stamp + 5 * STEP).revision == initial.revision + + +def test_commit_returns_detached_pose_and_diagnostics_for_exact_image(source): + tracker = SquareTagPoseTracker() + result = tracker.commit_motion_frame((selection(source, tracker, START),), establish=True) + old = result.observation_snapshots[0] + source.observe(tracker, START + STEP) + assert tracker.observation_snapshot("moving", START) is None + assert old.stamp_ns == START + assert old.diagnostics["observation_stamp_ns"] == START + assert old.diagnostics["branch_status"] == "tracking" + assert result.selections[0].pose == source.candidates[0] + with pytest.raises(FrozenInstanceError): + result.selections[0].snapshot.revision = 99 + + +def test_rejected_geometric_frame_reports_reason_and_preserves_reference(source): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + choice = selection(source, tracker, START + STEP) + reason = "pose_branch_motion_geometry_ambiguous" + result = tracker.reject_motion_frame((choice.snapshot,), reason) + assert not result.accepted and result.reason == reason + detail = result.observation_snapshots[0].diagnostics + assert detail["branch_reason"] == reason + assert detail["branch_revision"] == initial.revision + assert detail["motion_evidence_id"] == EVIDENCE + assert detail["branch_frozen"] is True + assert tracker.commit_motion_frame((choice,)).reason == "pose_observation_superseded" + next_choice = selection(source, tracker, START + 2 * STEP) + assert tracker.commit_motion_frame((next_choice,)).accepted + + +def test_late_geometric_rejection_cannot_modify_a_newer_frame(source): + tracker = SquareTagPoseTracker() + establish(source, tracker) + old = selection(source, tracker, START + STEP) + current = selection(source, tracker, START + 2 * STEP) + result = tracker.reject_motion_frame((old.snapshot,), "pose_branch_motion_geometry_ambiguous") + assert result.reason == "pose_observation_superseded" + assert tracker.commit_motion_frame((current,)).accepted + + +def test_rejection_batch_is_atomic_when_another_view_has_reset(source): + first, second = SquareTagPoseTracker(), SquareTagPoseTracker() + snapshots = (selection(source, first, START).snapshot, selection(source, second, START).snapshot) + second.reset() + result = first.reject_motion_frame(snapshots, "pose_branch_motion_geometry_inconsistent") + assert result.reason == "pose_observation_superseded" + assert first.observation_snapshot("moving", START)[1]["branch_reason"] == "pose_motion_evidence_required" + + +def test_geometric_rejection_cannot_hide_a_preexisting_frozen_conflict(source): + tracker = SquareTagPoseTracker() + initial = establish(source, tracker) + assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) + source.candidates = (pose(60., .05), pose(70., .2)) + for i in range(1, 6): + source.observe(tracker, START + i * STEP, managed=False) + assert tracker._branches.states["moving"].conflict + choice = selection(source, tracker, START + 6 * STEP) + result = tracker.reject_motion_frame((choice.snapshot,), "pose_branch_motion_geometry_inconsistent") + assert result.reason == "pose_branch_conflict" + assert result.observation_snapshots[0].diagnostics["branch_status"] == "frozen_conflict" diff --git a/src/linkerhand_calibration/test/test_motion_image_diagnostics.py b/src/linkerhand_calibration/test/test_motion_image_diagnostics.py new file mode 100644 index 0000000..039db3d --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_image_diagnostics.py @@ -0,0 +1,169 @@ +"""Image-space diagnosis stays separate from trusted branch/zero decisions.""" +from dataclasses import asdict, replace +import json + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( + MotionEvidenceFrame, MotionRelation, RoleCandidates, resolve_motion_branches, +) +from linkerhand_calibration.core.geometry.tag_pose.motion_image_diagnostics import ( + ImageMotionFrame, ImageRoleObservation, MotionImageDiagnosticParameters, analyze_motion_images, +) +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + +MATRIX = ((1600., 0., 640.), (0., 1600., 480.), (0., 0., 1.)) +RELATIONS = (MotionRelation("hinge", "root", "moving"),) + + +def _pose(rotation, translation): + return SquareTagPose(tuple(rotation.as_quat()), tuple(translation), .1) + + +def frames(*, chain=False, alternatives=False, depth_error=0., count=49): + result = [] + for index, theta in enumerate(np.linspace(0, np.deg2rad(80), count)): + root_rotation, root_translation = Rotation.identity(), np.array([0., 0., .8]) + rotation = Rotation.from_rotvec([0., 0., theta]) + translation = root_translation + [0.01, 0.02, 0.0] + rotation.apply([.035, 0., 0.]) + actual = [("root", root_rotation, root_translation), ("moving", rotation, translation)] + if chain: + child_rotation = rotation * Rotation.from_rotvec([theta * .45, 0., 0.]) + child_translation = translation + rotation.apply([.025, 0, 0]) + child_rotation.apply([0., .035, 0.]) + actual.append(("terminal", child_rotation, child_translation)) + roles, observations = [], [] + for role, rotation, translation in actual: + points = rotation.apply(square_object_points(.016)) + translation + pixels = points @ np.asarray(MATRIX).T + corners = pixels[:, :2] / pixels[:, 2:] + observations.append(ImageRoleObservation(role, .016, tuple(map(tuple, corners)))) + error = depth_error * np.sin(theta * 5) if role != "root" else 0. + pose = _pose(rotation, translation + [0., 0., error]) + candidates = (pose,) + if alternatives and role != "root": + wrong = _pose(Rotation.from_euler("y", 25, degrees=True) * rotation, + translation + [0., 0., error]) + candidates = (pose, wrong) + roles.append(RoleCandidates(role, candidates, frozen=role == "root")) + result.append(ImageMotionFrame(MotionEvidenceFrame(1_000_000_000 + index * 50_000_000, + tuple(roles)), MATRIX, tuple(observations))) + return tuple(result) + + +def test_image_fit_explains_noisy_depth_without_upgrading_branch_authorization(): + source = frames(depth_error=.008) + before = resolve_motion_branches([f.evidence for f in source], RELATIONS) + assert not before.resolved + report = analyze_motion_images(source, RELATIONS) + assert report.status == "available", report + assert report.scope == "diagnostic_not_branch_authorization" + assert not report.branch_authorization_allowed and not report.calibration_accuracy_verified + result = report.hypotheses[0] + assert result.pnp_geometry[0].translation_rms_m > .003 + assert result.pnp_geometry[0].camera_depth_squared_error_fraction > .99 + assert max(q.rms_px for interval in result.intervals for q in interval.validation_quality) < 1e-5 + assert resolve_motion_branches([f.evidence for f in source], RELATIONS) == before + json.dumps(asdict(report), allow_nan=False) + + +def test_unseen_shift_cannot_be_hidden_by_refitting_validation_geometry(): + source = list(frames()) + for index in range(len(source) // 2, len(source)): + observations = list(source[index].observations) + observations[1] = replace(observations[1], corners_xy=tuple( + (x + 8., y) for x, y in observations[1].corners_xy)) + source[index] = replace(source[index], observations=tuple(observations)) + report = analyze_motion_images(source, RELATIONS) + interval = next(v for v in report.hypotheses[0].intervals if v.name == "early_to_late") + assert interval.converged, interval + assert interval.training_quality[0].rms_px < 1e-5 + assert interval.validation_quality[0].rms_px > 2 + assert set(interval.training_stamps_ns).isdisjoint(interval.validation_stamps_ns) + + +def test_serial_graph_and_all_candidate_families_survive_as_diagnostics(): + source = frames(chain=True, alternatives=True) + relations = (*RELATIONS, MotionRelation("distal", "moving", "terminal")) + report = analyze_motion_images(source, tuple(reversed(relations))) + assert len(report.hypotheses) == 4 + assert all(dict(h.branches)["root"] == 0 for h in report.hypotheses) + assert any(h.intervals and h.intervals[0].converged for h in report.hypotheses) + assert not report.branch_authorization_allowed + for hypothesis in report.hypotheses: + for interval in hypothesis.intervals: + assert set(interval.training_stamps_ns).isdisjoint(interval.validation_stamps_ns) + assert len(interval.training_stamps_ns) <= 36 + assert len(interval.validation_stamps_ns) <= 72 + + +@pytest.mark.parametrize("change, reason", [ + ("missing", "image_observation_missing_or_duplicate"), + ("size", "tag_size_changed"), + ("matrix", "camera_model_changed"), + ("identity", "image_identity_invalid"), + ("unfrozen", "root_pose_not_frozen"), + ("nan", "tag_geometry_invalid"), +]) +def test_incomplete_or_changed_optics_are_explicitly_unavailable(change, reason): + source = list(frames()) + target = source[-1] + if change == "missing": + target = replace(target, observations=target.observations[:1]) + elif change == "size": + target = replace(target, observations=(target.observations[0], + replace(target.observations[1], tag_size_m=.017))) + elif change == "matrix": + target = replace(target, camera_matrix=((1500., 0., 640.), *MATRIX[1:])) + elif change == "identity": + target = replace(target, evidence=replace(target.evidence, stamp_ns=source[0].evidence.stamp_ns)) + elif change == "unfrozen": + target = replace(target, evidence=replace(target.evidence, roles=( + replace(target.evidence.roles[0], frozen=False), *target.evidence.roles[1:]))) + elif change == "nan": + target = replace(target, observations=(target.observations[0], replace(target.observations[1], + corners_xy=((float("nan"), 1.), *target.observations[1].corners_xy[1:])))) + source[-1] = target + report = analyze_motion_images(source, RELATIONS) + assert report.status == "not_available" and report.reason == reason + assert not report.hypotheses + + +def test_graph_and_solver_complexity_are_bounded(): + source = frames() + cyclic = (MotionRelation("a", "root", "moving"), MotionRelation("b", "moving", "root")) + assert analyze_motion_images(source, cyclic).reason == "cyclic_motion_graph" + params = MotionImageDiagnosticParameters(maximum_solver_evaluations=1) + report = analyze_motion_images(frames(depth_error=.003), RELATIONS, parameters=params) + assert report.status == "partial" + assert all(i.solver_evaluations <= 1 for i in report.hypotheses[0].intervals) + for kwargs in ({"maximum_hypotheses": 100}, {"maximum_solver_evaluations": 151}, + {"minimum_frames": 3}, {"maximum_relations": 10}): + with pytest.raises(ValueError): + MotionImageDiagnosticParameters(**kwargs) + + +def test_stationary_data_is_not_evidence_of_a_unique_image_branch(): + source = frames() + still = tuple(replace(source[0], evidence=replace(source[0].evidence, + stamp_ns=f.evidence.stamp_ns)) for f in source) + report = analyze_motion_images(still, RELATIONS) + assert report.status == "not_available" + assert report.hypotheses[0].reason == "insufficient_rotation_evidence" + + +def test_previously_frozen_moving_parent_uses_its_current_frame_pose(): + source = frames(chain=True) + changed = [] + for frame in source: + roles = tuple(replace(role, frozen=True) if role.role == "moving" else role + for role in frame.evidence.roles) + changed.append(replace(frame, evidence=replace(frame.evidence, roles=roles))) + report = analyze_motion_images(changed, (MotionRelation("distal", "moving", "terminal"),)) + assert report.status == "available", report + for interval in report.hypotheses[0].intervals: + assert interval.converged + assert interval.validation_quality[0].rms_px < 1e-5 + assert not report.branch_authorization_allowed diff --git a/src/linkerhand_calibration/test/test_motion_reference_provenance.py b/src/linkerhand_calibration/test/test_motion_reference_provenance.py new file mode 100644 index 0000000..6c24575 --- /dev/null +++ b/src/linkerhand_calibration/test/test_motion_reference_provenance.py @@ -0,0 +1,284 @@ +"""Motion authorization survives zero serialization, holdout and resume.""" + +from copy import deepcopy +from dataclasses import replace +import hashlib +import json + +import pytest + +from linkerhand_calibration.core.domain.reference import build_joint_zero_reference, read_joint_zero_references +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.joint_resume import JointResume +from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256, validate_motion_provenance +from test_joint_zero_runtime import reference_rows + + +def motion_capture(*, zero_stamp=100, revision=7, source_stamps=(10, 20), layout="o6_right_8", task_index=0): + profile = load_bundled_hand_profile(layout) + task = profile.motion.tasks[task_index] + name = task.joints[0] + profile = replace(profile, motion=replace(profile.motion, tasks=(task,))) + measurement = profile.measurement.measurements[name] + role = measurement.child_role + payload = {"task_name": task.key, "view": measurement.view, "tag_role": role, + "zero_joints": [name], "session_epoch": 1, "motion_version": 1, + "source_image_stamps": list(source_stamps), "constraints": [{"child_role": role, "axis": [1., 0., 0.]}]} + sources = [{"kind": "motion_branch_observation", "image_stamp_ns": stamp, + "command_vector": list(profile.command.baseline_values), "feedback_vector": list(profile.command.baseline_values), + "tags": {role: {"corners_xy": [[1., 1.], [2., 1.], [2., 2.], [1., 2.]], + "reprojection_valid_candidates": [{"quaternion_xyzw": [0., 0., 0., 1.], + "translation_xyz_m": [0., 0., .5]}]}}, + **{key: payload[key] for key in ("task_name", "view", "zero_joints", "session_epoch", "motion_version")}} + for stamp in source_stamps] + payload["source_frame_hashes"] = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in sources} + identity = hashlib.sha256(json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() + audit = {"kind": "motion_branch_initialization", "status": "resolved", "reason": "", + **{key: value for key, value in payload.items() if key != "tag_role"}, + "evidence_ids": {role: identity}, "evidence_payloads": {role: payload}} + zeros = reference_rows(profile, name, stamp=zero_stamp) + for row in zeros: + row.update(kind="joint_zero_sample", joint=name) + row["pnp_observation_evidence"]["child"]["candidate_diagnostics"].update( + branch_revision=revision, motion_evidence_verified=True, motion_evidence_id=identity, branch_frozen=False) + reference = build_joint_zero_reference(profile, name, zeros, session_epoch=1, motion_version=2) + scans = [] + for cycle in range(4): + row = deepcopy(zeros[-1]) + row.update(kind="joint_sample", sample_phase="sweep", cycle=cycle, image_stamp_ns=zero_stamp+20+cycle) + for item in row["pnp_observation_evidence"].values(): + item["candidate_diagnostics"].update(observation_stamp_ns=row["image_stamp_ns"], branch_frozen=True) + scans.append(row) + return profile, reference, [*sources, audit, *zeros, reference.as_record(), *scans] + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def test_motion_zero_roundtrip_and_all_four_cycles_remain_bound_without_header(layout): + profile, reference, records = motion_capture(layout=layout) + restored = read_joint_zero_references(profile, records)[reference.joint] + assert restored == reference + assert len(restored.branch_references) == 1 + report = validate_motion_provenance(profile, records, require_complete=True) + assert report["motion_evidence_verified"] is True + assert report["is_accuracy_certificate"] is False + + +@pytest.mark.parametrize("mutation", ["missing_audit", "missing_source", "model_tampered", "sample_id", + "holdout_revision", "holdout_not_frozen", "zero_provenance_removed", "false_verification"]) +def test_missing_or_changed_provenance_is_rejected_even_without_policy_header(mutation): + profile, reference, records = motion_capture() + if mutation == "missing_audit": + records = [row for row in records if row["kind"] != "motion_branch_initialization"] + elif mutation == "missing_source": + records.pop(0) + elif mutation == "model_tampered": + audit = next(row for row in records if row["kind"] == "motion_branch_initialization") + next(iter(audit["evidence_payloads"].values()))["constraints"][0]["axis"] = [0., 1., 0.] + elif mutation == "zero_provenance_removed": + next(row for row in records if row["kind"] == "joint_zero_reference").pop("branch_references") + else: + diagnostics = records[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"] + field, value = {"sample_id": ("motion_evidence_id", "a"*64), + "holdout_revision": ("branch_revision", 8), "holdout_not_frozen": ("branch_frozen", False), + "false_verification": ("motion_evidence_verified", False)}[mutation] + diagnostics[field] = value + with pytest.raises(ValueError, match="motion_branch_provenance_"): + validate_motion_provenance(profile, records, require_complete=True) + + +def test_motion_support_must_precede_first_frozen_zero_image(): + profile, _, records = motion_capture(source_stamps=(100, 101)) + with pytest.raises(ValueError, match="initialization_not_before_zero"): + validate_motion_provenance(profile, records) + + +@pytest.mark.parametrize("field", ["candidate", "corners", "command_vector", "feedback_vector"]) +def test_source_frame_content_cannot_change_without_invalidating_motion_evidence(field): + profile, _, records = motion_capture() + frame = records[0] + if field in {"command_vector", "feedback_vector"}: + frame[field][0] += 1 + else: + tag = next(iter(frame["tags"].values())) + if field == "candidate": + tag["reprojection_valid_candidates"][0]["translation_xyz_m"][0] += .01 + else: + tag["corners_xy"][0][0] += 1 + with pytest.raises(ValueError, match="source_content_changed"): + validate_motion_provenance(profile, records) + + +@pytest.mark.parametrize("mutation", ["missing", "extra"]) +def test_source_hashes_must_cover_exactly_the_declared_support_frames(mutation): + profile, _, records = motion_capture() + audit = next(row for row in records if row["kind"] == "motion_branch_initialization") + # Mutate only the top-level declaration so the immutable payload still + # independently detects its binding; it cannot authorize a subset instead. + audit["source_frame_hashes"] = dict(audit["source_frame_hashes"]) + if mutation == "missing": + audit["source_frame_hashes"].pop("10") + else: + audit["source_frame_hashes"]["999"] = "a"*64 + with pytest.raises(ValueError, match="initialization_binding_changed|source_hash_coverage_invalid"): + validate_motion_provenance(profile, records) + + +def test_zero_cannot_mix_different_authorizations_across_reference_images(): + profile, reference, records = motion_capture() + rows = [row for row in records if row["kind"] == "joint_zero_sample"] + rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["motion_evidence_id"] = "a"*64 + with pytest.raises(ValueError, match="joint_zero_motion_evidence_changed"): + build_joint_zero_reference(profile, reference.joint, rows, session_epoch=1, motion_version=2) + + +def test_legacy_zero_remains_readable_without_claiming_motion_confirmation(): + profile, reference, _ = motion_capture() + legacy = build_joint_zero_reference(profile, reference.joint, + reference_rows(profile, reference.joint, stamp=100), session_epoch=1, motion_version=2) + row = legacy.as_record() + row.pop("branch_references") + assert read_joint_zero_references(profile, [row])[reference.joint].branch_references == () + assert validate_motion_provenance(profile, [row])["motion_evidence_verified"] is False + with pytest.raises(ValueError, match="zero_evidence_missing"): + validate_motion_provenance(profile, [row], required=True) + with pytest.raises(ValueError, match="zero_evidence_missing"): + validate_motion_provenance(profile, [{"kind": "session_start", + "pose_tracking_policy_version": "confirmed_branch_v4_motion_geometry"}, row]) + + +def test_legacy_branch_events_are_diagnostics_and_new_events_cannot_downgrade(): + profile, _, _ = motion_capture() + event = {"kind": "pnp_branch_event", "pnp_observation_evidence": { + "tag_role": "thumb_yaw", "corners_xy": [[0, 0]]*4, + "candidate_diagnostics": {"branch_frozen": True}}} + assert validate_motion_provenance(profile, [event])["motion_evidence_verified"] is False + event["pnp_observation_evidence"]["candidate_diagnostics"]["motion_evidence_verified"] = True + with pytest.raises(ValueError, match="joint_zero_references_missing"): + validate_motion_provenance(profile, [event]) + + +def test_motion_confirmation_does_not_require_fixed_tag_motion_evidence(): + profile, _, records = motion_capture() + for row in records: + for side, item in row.get("pnp_observation_evidence", {}).items(): + if side == "parent": + item["candidate_diagnostics"].update(motion_evidence_verified=False, motion_evidence_id="") + assert validate_motion_provenance(profile, records, require_complete=True)["motion_evidence_verified"] is True + + +def test_repeated_physical_tag_keeps_one_original_evidence_payload(): + profile, _, records = motion_capture() + first = next(row for row in records if row["kind"] == "motion_branch_initialization") + repeated = {**first, "task_name": "later_task", "source_image_stamps": [500], + "evidence_payloads": {}} + source = {**records[0], "task_name": "later_task", "image_stamp_ns": 500} + repeated["source_frame_hashes"] = {"500": source_frame_sha256(source)} + records.append(source) + records.append(repeated) + assert validate_motion_provenance(profile, records)["evidence_count"] == 1 + + +def test_resume_rechecks_pose_and_preserves_old_zero_with_distinct_new_evidence_ids(): + profile, previous, original = motion_capture() + _, current, fresh = motion_capture(zero_stamp=300, revision=9, source_stamps=(210, 220)) + resume = JointResume(profile) + resume.references = {previous.joint: previous} + resume.rows = tuple(original) + assert resume.verify(current) is previous + authorized = resume.take_motion_provenance_records() + assert any(row["kind"] == "joint_zero_motion_reference_verified" for row in authorized) + assert resume.take_motion_provenance_records() == () + merged = [row for row in fresh if row["kind"] != "joint_zero_reference"] + merged.extend(authorized) + merged.append(previous.as_record()) + assert validate_motion_provenance(profile, merged, require_complete=True)["motion_evidence_verified"] is True + mapping = next(row for row in merged if row["kind"] == "joint_zero_motion_reference_verified") + # Swapping an unrelated held pose cannot authorize reuse merely because IDs exist. + mapping["current_reference"]["samples"][0]["parent_pose_common"]["translation_xyz_m"] = (1., 0., .5) + with pytest.raises(ValueError): + validate_motion_provenance(profile, merged) + + +def test_resume_imports_source_evidence_once_but_keeps_each_joint_verification(): + # G20 first two tasks each observe one independent motion role. This test + # checks journal delivery across joints, not the geometry solver itself. + profile = load_bundled_hand_profile("g20_right_19") + _, first, first_rows = motion_capture(layout="g20_right_19") + _, second, second_rows = motion_capture(layout="g20_right_19", task_index=1, + zero_stamp=200, source_stamps=(110, 120)) + resume = JointResume(profile) + resume.references = {first.joint: first, second.joint: second} + resume.rows = tuple([*first_rows, *second_rows]) + assert resume.verify(first) is first + first_batch = resume.take_motion_provenance_records() + assert any(row["kind"] == "motion_branch_observation" for row in first_batch) + assert resume.verify(second) is second + second_batch = resume.take_motion_provenance_records() + assert all(row["kind"] in {"joint_zero_sample", "joint_zero_motion_reference_verified"} for row in second_batch) + assert all(row["joint"] == second.joint for row in second_batch) + assert sum(row["kind"] == "joint_zero_motion_reference_verified" for row in second_batch) == 1 + assert resume.verify(first) is first + assert resume.take_motion_provenance_records() == () + # Failed group freezing may defer draining; retry replaces the pending + # verification and still does not copy the checkpoint sources again. + later = replace(second, motion_version=second.motion_version+1) + resume.verify(later) + resume.verify(later) + final_batch = resume.take_motion_provenance_records() + assert len(final_batch) == 1 and final_batch[0]["joint"] == second.joint + + +def test_partial_resume_of_an_already_resumed_session_keeps_unverified_joint_mappings_out(): + from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION + from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine + + profile = load_bundled_hand_profile("g20_right_19") + _, first, first_rows = motion_capture(layout="g20_right_19") + _, second, second_rows = motion_capture(layout="g20_right_19", task_index=3, + zero_stamp=200, source_stamps=(110, 120)) + original = {first.joint: first, second.joint: second} + previous = JointResume(profile) + previous.references, previous.rows = original, tuple([*first_rows, *second_rows]) + previous_session = [] + for index, stamp in ((0, 1000), (3, 1200)): + _, current, fresh = motion_capture(layout="g20_right_19", task_index=index, + zero_stamp=stamp, source_stamps=(stamp-100, stamp-90)) + previous.verify(current) + previous_session.extend(row for row in fresh if row["kind"] != "joint_zero_reference") + previous_session.extend(previous.take_motion_provenance_records()) + previous_session.append(original[current.joint].as_record()) + assert validate_motion_provenance(profile, previous_session, required=True)["motion_evidence_verified"] is True + header = {"kind": "session_start", "profile_id": profile.key.profile_id, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version} + resuming = JointResume(profile) + resuming.stage(CalibrationEngine(profile), [header, *previous_session], set()) + _, current, fresh = motion_capture(layout="g20_right_19", zero_stamp=2000, source_stamps=(1900, 1910)) + preserved = resuming.verify(current) + partial = [header, *(row for row in fresh if row["kind"] != "joint_zero_reference"), + *resuming.take_motion_provenance_records(), preserved.as_record()] + assert all(row.get("joint") == first.joint for row in partial + if row.get("kind") in {"joint_zero_sample", "joint_zero_motion_reference_verified"}) + assert validate_motion_provenance(profile, partial, required=True)["joint_count"] == 1 + # An abort at this point must leave a valid next checkpoint. It must not + # require the second joint, whose installation has not been checked yet. + subsequent = JointResume(profile) + subsequent.stage(CalibrationEngine(profile), partial, set()) + assert set(subsequent.references) == {first.joint} + + +def test_finalizer_requires_provenance_for_task_outside_articulated_candidate_selection(tmp_path, monkeypatch): + from pathlib import Path + from linkerhand_calibration.runtime.artifacts import finalization + _, _, records = motion_capture() + profile = load_bundled_hand_profile("o6_right_8") + records = [row for row in records if row["kind"] != "motion_branch_initialization"] + profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset())) + source = Path(__file__).resolve().parents[1]/"urdf/o6_right/linkerhand_o6_right.urdf" + monkeypatch.setattr(finalization, "fit_profile_calibration", lambda *a, **kw: pytest.fail("fit must not start")) + with pytest.raises(ValueError, match="motion_branch_provenance_"): + finalization.finalize_profile_session(profile=profile, session_dir=tmp_path, + serial_number="TEST", source_urdf=source, protected_inputs={}, records=records, + require_motion_evidence=True) + assert not list(tmp_path.glob("*.urdf")) diff --git a/src/linkerhand_calibration/test/test_motion_transfer.py b/src/linkerhand_calibration/test/test_motion_transfer.py index c1e5644..65d6894 100644 --- a/src/linkerhand_calibration/test/test_motion_transfer.py +++ b/src/linkerhand_calibration/test/test_motion_transfer.py @@ -36,6 +36,15 @@ def _digest(path): return hashlib.sha256(path.read_bytes()).hexdigest() +def _geometry_only_o6_profile(): + """Keep default CAD safety bounds for metadata-only structure fixtures. + + The production nominal-envelope policy needs actual training/reference + evidence. Full measured O6 capture tests cover that separate contract. + """ + return replace(load_bundled_hand_profile("o6_right_8"), urdf_limit_policies={}) + + def _frozen_fit(profile): observed = {name for name, spec in profile.measurement.measurements.items() if spec.kind != "validation_only" and name in profile.motion.joint_zero_references} @@ -147,6 +156,9 @@ def test_old_zero_and_mimic_transfer_declarations_do_not_authorize_v3_motion_cop def test_o12_motion_transfer_does_not_invent_passive_pip_zero(): profile = load_bundled_hand_profile("o12_right_16") + profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions={ + name: value for name, value in profile.zero.cad_zero_assumptions.items() + if name not in {"pinky_pip", "ring_pip", "thumb_mcp"}})) expanded = expand_profile_transfers(profile, _frozen_fit(profile)) for name in ("pinky_pip", "ring_pip"): assert name in expanded.output_mappings @@ -156,8 +168,23 @@ def test_o12_motion_transfer_does_not_invent_passive_pip_zero(): prepare_standard_result(profile, O12_SOURCE, _digest(O12_SOURCE), expanded) +def test_o12_declared_cad_baselines_do_not_become_measured_zero_evidence(): + profile = load_bundled_hand_profile("o12_right_16") + fit = expand_profile_transfers(profile, _frozen_fit(profile)) + for name in ("thumb_mcp", "pinky_pip", "ring_pip"): + assert name in profile.zero.cad_zero_assumptions + assert name not in profile.zero.direct_zero_joints + assert fit.zero_offsets_rad[name] == 0 + assert fit.zero_method_by_joint[name] == "assumed_source_cad_zero" + assert fit.command_mappings["ring_pip"].transferred_from == "pinky_pip" + assert "ring_pip" not in fit.joint_zero_references + + def test_passive_pip_zero_uses_separate_datums_while_motion_is_copied(): profile = load_bundled_hand_profile("o12_right_16") + profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions={ + name: value for name, value in profile.zero.cad_zero_assumptions.items() + if name not in {"pinky_pip", "ring_pip"}})) geometry = dict(profile.zero.known_baseline_geometry, pinky_pip=KnownBaselineGeometry(.07, .001, "synthetic_pinky_datum")) profile = replace(profile, zero=replace(profile.zero, known_baseline_geometry=geometry)) @@ -176,7 +203,7 @@ def test_passive_pip_zero_uses_separate_datums_while_motion_is_copied(): def test_o6_urdf_keeps_terminal_origins_and_each_recipient_link_geometry(tmp_path): - profile = load_bundled_hand_profile("o6_right_8") + profile = _geometry_only_o6_profile() expanded = expand_profile_transfers(profile, _frozen_fit(profile)) source_hash = _digest(O6_SOURCE) prepared = prepare_standard_result(profile, O6_SOURCE, source_hash, expanded) @@ -187,7 +214,9 @@ def test_o6_urdf_keeps_terminal_origins_and_each_recipient_link_geometry(tmp_pat for name in profile.zero.cad_zero_assumptions: assert expanded.zero_offsets_rad[name] == 0. assert after_joints[name].find("origin").attrib == before_joints[name].find("origin").attrib - assert prepared.plan.mimic_output[name][2] == 0. + assert after_joints[name].find("mimic").get("joint") == before_joints[name].find("mimic").get("joint") + assert prepared.plan.mimic_policy == "preserve_source_mimic" + assert after_joints[name].find("mimic").attrib == before_joints[name].find("mimic").attrib for name, original in before_joints.items(): current = after_joints[name] for tag in ("parent", "child", "axis"): @@ -210,7 +239,9 @@ def test_changed_recipient_curve_cannot_pass_as_exact_transfer(field): def test_transferred_curve_cannot_expand_recipient_mechanical_range(tmp_path): - profile = load_bundled_hand_profile("o6_right_8") + # Without an explicit nominal-envelope declaration, the recipient's own + # CAD upper bound remains a mandatory physical limit after zero conversion. + profile = _geometry_only_o6_profile() expanded = expand_profile_transfers(profile, _frozen_fit(profile)) source = tmp_path/"narrow_recipient.urdf" tree = ET.parse(O6_SOURCE) @@ -220,6 +251,14 @@ def test_transferred_curve_cannot_expand_recipient_mechanical_range(tmp_path): prepare_standard_result(profile, source, _digest(source), expanded) +def test_metadata_only_transfer_fit_cannot_authorize_measured_motion_envelopes(): + profile = load_bundled_hand_profile("o6_right_8") + expanded = expand_profile_transfers(profile, _frozen_fit(profile)) + assert profile.urdf_limit_policies + with pytest.raises(ValueError, match="motion_range_requires_training_provenance"): + prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), expanded) + + @pytest.mark.parametrize("problem", ["missing_donor", "wrong_channel", "already_expanded"]) def test_invalid_donor_result_cannot_be_transferred(problem): profile = load_bundled_hand_profile("o6_right_8") @@ -244,7 +283,7 @@ def test_invalid_donor_result_cannot_be_transferred(problem): ]) @pytest.mark.parametrize("changed", ["offset", "method"]) def test_result_plan_rejects_zero_parameters_that_change_the_declared_evidence(policy, joint, reason, changed): - profile = load_bundled_hand_profile("o6_right_8") + profile = _geometry_only_o6_profile() if policy == "known": assumptions = {name: value for name, value in profile.zero.cad_zero_assumptions.items() if name != joint} geometry = dict(profile.zero.known_baseline_geometry, diff --git a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py index b724fb3..e15bfd3 100644 --- a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py +++ b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py @@ -90,8 +90,6 @@ def test_profile_authorizes_every_observed_active_zero_without_endpoint_assumpti from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import get_right_19_thumb_zero_profile assert not get_right_19_thumb_zero_profile().accept_validated_zero_in_confidence_interval assert full_hand_zero_profile().accept_validated_zero_in_confidence_interval - assert not get_right_19_thumb_zero_profile().project_axis_gauge_before_image - assert full_hand_zero_profile().project_axis_gauge_before_image assert full_hand_zero_profile().hand.stable_cross_view_cone_bias model = UrdfKinematicModel(SOURCE_URDF) for child, parent in PHASE_PARENT.items(): @@ -229,6 +227,10 @@ def test_failed_spatial_solve_writes_evidence_without_replacing_published_result diagnostic = {"passed": False, "stage": "axis_observation", "joint": "thumb_mcp"} def fail(*args, **kwargs): raise O12SpatialZeroError("unreliable geometry", diagnostic) + # This test isolates failure after the image-evidence boundary; frozen + # branch acceptance and rejection are exercised with actual candidates. + monkeypatch.setattr(finalization, "resolve_chain_observations", + lambda rows, **kwargs: (rows, {"status": "resolved"})) monkeypatch.setattr(finalization, "fit_profile_calibration", fail) session = tmp_path / "new" with pytest.raises(O12SpatialZeroError): diff --git a/src/linkerhand_calibration/test/test_o12_observation_resolution.py b/src/linkerhand_calibration/test/test_o12_observation_resolution.py index b999eb6..67a30c2 100644 --- a/src/linkerhand_calibration/test/test_o12_observation_resolution.py +++ b/src/linkerhand_calibration/test/test_o12_observation_resolution.py @@ -1,4 +1,5 @@ import copy +from pathlib import Path import numpy as np import pytest from scipy.spatial.transform import Rotation as R @@ -6,6 +7,7 @@ from scipy.spatial.transform import Rotation as R from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12 import observations as module from linkerhand_calibration.pnp import SquareTagPose +SOURCE_URDF = Path(__file__).resolve().parents[1]/"urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" def build_data(monkeypatch, *, wrong_holdout=False): """Ambiguous initial poses: the wrong distal branch has lower image error.""" @@ -89,7 +91,7 @@ def test_other_task_evidence_is_not_mixed_into_thumb(monkeypatch): def test_partial_external_projection_cannot_finalize_whole_hand(tmp_path): from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.pipeline import finalize_o12_session with pytest.raises(ValueError,match='diagnostic-only'): - finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused', + finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf=SOURCE_URDF, protected_inputs={},records=[{'projection_reprocessing_scope':'thumb_only_external_projection'}],publish=False) @@ -100,7 +102,7 @@ def test_known_unverified_projection_cannot_publish_even_if_fit_passes(monkeypat raise AssertionError('unverified projection must be rejected before fitting') monkeypatch.setattr(finalization,'fit_profile_calibration',must_not_fit) with pytest.raises(ValueError, match='camera_projection_unverified'): - pipeline.finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused', + pipeline.finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf=SOURCE_URDF, protected_inputs={},records=[{'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','image_stamp_ns':1}]) assert not (tmp_path/'latest_passed').exists() diff --git a/src/linkerhand_calibration/test/test_o12_right_profile.py b/src/linkerhand_calibration/test/test_o12_right_profile.py index b876c20..38dff29 100644 --- a/src/linkerhand_calibration/test/test_o12_right_profile.py +++ b/src/linkerhand_calibration/test/test_o12_right_profile.py @@ -259,15 +259,18 @@ def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: def test_o12_vendor_stream_is_not_artificially_throttled() -> None: - sdk_config = ( - ROOT.parent - / "agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node" - / "config/omnihand_pro_2025_node.yaml" - ).read_text() - active_right = sdk_config.split("right_hand:", 1)[1].split( - "# Alternative", 1 - )[0] - assert "request_interval_ms: 0" in active_right + import hashlib + import yaml + from linkerhand_calibration.compat.paths import resolve_renamed_package_path + + product = yaml.safe_load((ROOT/"config/o12_right_product.yaml").read_text()) + sdk_path = resolve_renamed_package_path(product["sdk"]["config"], ROOT.parent.parent) + assert sdk_path == ROOT/"config/o12_sdk.yaml" + assert hashlib.sha256(sdk_path.read_bytes()).hexdigest() == product["sdk"]["config_sha256"] + parameters = yaml.safe_load(sdk_path.read_text())["/**"]["ros__parameters"] + assert "left_hand" not in parameters + assert parameters["right_hand"]["request_interval_ms"] == 0 + assert parameters["right_hand"]["connection_type"] == product["sdk"]["transport"] def test_o12_error_health_separates_active_faults_from_historical_comm_latch( diff --git a/src/linkerhand_calibration/test/test_o6_tag_installation.py b/src/linkerhand_calibration/test/test_o6_tag_installation.py new file mode 100644 index 0000000..d489917 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o6_tag_installation.py @@ -0,0 +1,25 @@ +"""The top thumb Tag measures yaw but is physically on the distal link.""" + +from pathlib import Path + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.profiles.observations import compile_tag_links + + +def test_top_thumb_tag_shares_the_distal_link_and_follows_flexion(): + package=Path(__file__).resolve().parents[1] + config=load_product_config(package/"config/o6_right_product.yaml",check_can=False) + profile=config.calibration_contract.typed_profile + model=UrdfKinematicModel(config.source_urdf) + links=compile_tag_links(profile,model) + assert links["thumb_yaw"]==links["thumb_ip"]=="rh_thumb_distal" + joint=model.parent_joint_by_child[links["thumb_yaw"]] + before=model.link_transform(joint,zero_offsets={},joint_angles={},independent_mimic_angles=True) + angles={"rh_thumb_cmc_yaw":0.,"rh_thumb_cmc_pitch":.35,"rh_thumb_ip":.45} + after=model.link_transform(joint,zero_offsets={},joint_angles=angles,independent_mimic_angles=True) + assert np.linalg.norm(after[:3,3]-before[:3,3])>.01 + assert Rotation.from_matrix(before[:3,:3].T@after[:3,:3]).magnitude()>.3 diff --git a/src/linkerhand_calibration/test/test_observation_capture.py b/src/linkerhand_calibration/test/test_observation_capture.py index ed1c8a7..6afb19e 100644 --- a/src/linkerhand_calibration/test/test_observation_capture.py +++ b/src/linkerhand_calibration/test/test_observation_capture.py @@ -38,7 +38,7 @@ def frame(profile, stamp, *, omit=(), shift=0, feedback=True): 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]]), + return CaptureFrame("front", stamp*40_000_000, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]), corners, values if feedback else None, values, command_directions=("increasing",)*len(values)) @@ -53,7 +53,8 @@ def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_si for _ in range(20): capture.consume(frame(profile, 100), collect_installations=True) assert not lock.references - for index in range(9): + # Five independent images confirm a branch; ten accepted images lock it. + for index in range(13): capture.consume(frame(profile, 101+index), collect_installations=True) fixed, moving = capture.fingerprint_poses() assert "front" in fixed @@ -66,7 +67,7 @@ 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): + for i in range(14): 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") @@ -85,7 +86,7 @@ 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): + for i in range(14): capture.consume(frame(profile, i+1)) for i in range(9): assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None @@ -97,7 +98,7 @@ def test_joint_zero_only_requires_its_own_parent_child_tags(): profile, lock, capture = make_capture() lock.begin_session() lock.start_locking() - for i in range(10): + for i in range(14): capture.consume(frame(profile, i+1)) name = "rh_thumb_cmc_pitch" spec = profile.motion.joint_zero_references[name] @@ -120,7 +121,7 @@ def test_articulated_capture_discards_partial_moving_tags_and_records_cached_bas capture.profile = profile lock.begin_session() lock.start_locking() - for i in range(10): + for i in range(14): capture.consume(frame(profile, i+1)) motion = MotionCommand("sweep", profile.command.baseline_values, 1, "thumb_pitch_dip_front", 0, 0, "decreasing") diff --git a/src/linkerhand_calibration/test/test_observation_strategy.py b/src/linkerhand_calibration/test/test_observation_strategy.py index 0726f40..0f23adf 100644 --- a/src/linkerhand_calibration/test/test_observation_strategy.py +++ b/src/linkerhand_calibration/test/test_observation_strategy.py @@ -17,8 +17,7 @@ PACKAGE = Path(__file__).resolve().parents[1] PROFILES = ( ("o6_right_8", "o6_right/linkerhand_o6_right.urdf", 6, ()), ("l6_right_8", "l6_right/linkerhand_l6v3.1_right.urdf", 6, ()), - ("o12_right_16", "o12_right/linkerhand_o12_t3_right-0703.urdf", 3, - ("pinky_pip", "ring_pip", "thumb_mcp")), + ("o12_right_16", "o12_right/linkerhand_o12_t3_right-0703.urdf", 3, ()), ("g20_right_19", "g20_right/linkerhand_g20_right.urdf", 0, ()), ) @@ -42,6 +41,9 @@ def test_common_strategy_retains_measured_coverage_and_explicit_gaps(layout, urd product = yaml.safe_load((PACKAGE / "config" / f"{profile.key.model.lower()}_right_product.yaml").read_text()) assert product["profile_config_sha256"] == hashlib.sha256( (PACKAGE / "config" / "profiles" / f"{layout}.yaml").read_bytes()).hexdigest() + from linkerhand_calibration.profiles.validator import require_release_geometry + require_release_geometry(report) + assert profile.acquisition.command_capture_mode == "separate" def test_old_geometry_transfer_does_not_authorize_copying_motion(): @@ -52,6 +54,9 @@ def test_old_geometry_transfer_does_not_authorize_copying_motion(): assert len(coverage["unresolved_motion_joints"]) == 6 assert coverage["transferred_motion_sources"] == {} assert set(coverage["missing_baseline_geometry"]) == set(profile.zero.transferred_zero_sources) + from linkerhand_calibration.profiles.validator import require_release_geometry + with pytest.raises(ValueError, match="release_geometry_incomplete"): + require_release_geometry({"independent_measurement_coverage": coverage}) @pytest.mark.parametrize("target,source", [ diff --git a/src/linkerhand_calibration/test/test_observation_timing.py b/src/linkerhand_calibration/test/test_observation_timing.py new file mode 100644 index 0000000..25aed78 --- /dev/null +++ b/src/linkerhand_calibration/test/test_observation_timing.py @@ -0,0 +1,134 @@ +"""Report image loss separately from visibility and synchronization failures.""" + +from copy import deepcopy + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.observation_timing import summarize_observation_timing + + +@pytest.fixture +def capture(): + profile = load_bundled_hand_profile("o6_right_8") + unit = next(unit for unit in CalibrationEngine(profile).scan_units() + if unit.task_key == "thumb_pitch_ip_front") + tags = next(view.tags for view in profile.vision.views if view.name == "front") + rows = [] + for index in range(5): + stamp = 1_000_000_000 + index * 100_000_000 + rows.append({"kind": "diagnostic_observation_frame", "sample_phase": "sweep", + "task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction, + "attempt": 1, "view": "front", "image_stamp_ns": stamp, + "command_unit": profile.command.unit, + "command_vector": list(profile.command.baseline_values), + "feedback_vector": list(profile.command.baseline_values), + "tags": {tag.role: {"tag_role": tag.role, "tag_id": tag.tag_id, + "corners_xy": [[10, 10], [50, 10], [50, 50], [10, 50]], + "candidate_diagnostics": {"observation_stamp_ns": stamp}} + for tag in tags if not tag.fixed_reference}}) + return profile, unit, rows + + +def ip_metrics(capture, rows=None): + profile, unit, original = capture + result = summarize_observation_timing(profile, unit, 1, original if rows is None else rows) + return result[f"joint:rh_thumb_ip:front:{unit.direction}"] + + +def test_current_moving_tags_are_visible_without_fixed_tag_or_selected_pose(capture): + before = deepcopy(capture[2]) + metrics = ip_metrics(capture) + assert metrics == {"raw_frames": 5, "visible_frames": 5, "synchronized_frames": 5, + "missing_command_frames": 0, "missing_feedback_frames": 0, + "maximum_image_gap_seconds": 0.1, "maximum_synchronized_gap_seconds": 0.1} + assert capture[2] == before + assert "passed" not in metrics + + +def test_command_and_feedback_gaps_do_not_disappear_into_image_counts(capture): + rows = capture[2] + rows[1]["command_vector"] = None + rows[2]["feedback_vector"] = None + rows[3]["command_vector"] = [] + rows[3]["feedback_vector"] = [float("nan")] * len(rows[3]["feedback_vector"]) + metrics = ip_metrics(capture) + assert metrics["raw_frames"] == metrics["visible_frames"] == 5 + assert metrics["synchronized_frames"] == 2 + assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 2 + assert metrics["maximum_image_gap_seconds"] == 0.1 + assert metrics["maximum_synchronized_gap_seconds"] == 0.4 + + +@pytest.mark.parametrize("role", ["thumb_pitch", "thumb_ip"]) +@pytest.mark.parametrize("fault", ["missing", "stale", "id", "role", "corners", "not_finite"]) +def test_both_moving_tags_need_current_correctly_bound_corners(capture, role, fault): + rows = capture[2] + for row in rows[1:4]: + item = row["tags"][role] + if fault == "missing": + del row["tags"][role] + elif fault == "stale": + item["candidate_diagnostics"]["observation_stamp_ns"] -= 1 + elif fault == "id": + item["tag_id"] = -1 + elif fault == "role": + item["tag_role"] = "unrelated" + elif fault == "corners": + item["corners_xy"] = None + else: + item["corners_xy"][0][0] = float("inf") + metrics = ip_metrics(capture) + assert metrics["raw_frames"] == 5 + assert metrics["visible_frames"] == metrics["synchronized_frames"] == 2 + assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 0 + assert metrics["maximum_synchronized_gap_seconds"] == 0.4 + + +def test_duplicate_images_are_counted_once_and_not_merged(capture): + profile, unit, rows = capture + original = deepcopy(rows) + assert ip_metrics(capture, rows + deepcopy(rows)) == ip_metrics(capture, original) + rows[2]["command_vector"] = None + duplicates = deepcopy(rows) + duplicates[2]["command_vector"] = list(profile.command.baseline_values) + duplicates[2]["feedback_vector"] = None + metrics = ip_metrics(capture, rows + duplicates) + assert metrics["raw_frames"] == 5 + assert metrics["synchronized_frames"] == 4 + assert metrics["missing_command_frames"] == 1 + assert metrics["maximum_synchronized_gap_seconds"] == 0.2 + + +@pytest.mark.parametrize("field,value", [ + ("kind", "joint_sample"), ("sample_phase", "joint_zero"), + ("task_name", "thumb_yaw_top"), ("cycle", 1), ("direction", "increasing"), + ("attempt", 2), ("view", "side"), ("image_stamp_ns", None), + ("image_stamp_ns", 0), ("image_stamp_ns", True), +]) +def test_other_scopes_and_invalid_image_identities_do_not_supply_frames(capture, field, value): + for row in capture[2]: + row[field] = value + metrics = ip_metrics(capture) + assert metrics["raw_frames"] == metrics["visible_frames"] == metrics["synchronized_frames"] == 0 + assert metrics["maximum_image_gap_seconds"] is None + assert metrics["maximum_synchronized_gap_seconds"] is None + + +@pytest.mark.parametrize("count", [0, 1]) +def test_fewer_than_two_images_cannot_establish_a_gap(capture, count): + metrics = ip_metrics(capture, capture[2][:count]) + assert metrics["raw_frames"] == count + assert metrics["maximum_image_gap_seconds"] is None + assert metrics["maximum_synchronized_gap_seconds"] is None + + +def test_incorrect_native_unit_is_not_a_synchronized_vector(capture): + for row in capture[2]: + row["command_unit"] = "rad" + metrics = ip_metrics(capture) + assert metrics["raw_frames"] == metrics["visible_frames"] == 5 + assert metrics["synchronized_frames"] == 0 + assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 5 + assert metrics["maximum_synchronized_gap_seconds"] is None diff --git a/src/linkerhand_calibration/test/test_paused_snapshot.py b/src/linkerhand_calibration/test/test_paused_snapshot.py new file mode 100644 index 0000000..8f343c6 --- /dev/null +++ b/src/linkerhand_calibration/test/test_paused_snapshot.py @@ -0,0 +1,106 @@ +"""A paused task must never display another scan's successful statistics.""" + +import pytest + +from linkerhand_calibration.runtime.engine import SweepQuality +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.status import render_status_zh +from runtime_host_fixture import coordinator_fixture, ready + + +@pytest.mark.parametrize('current_samples', [0, 37]) +def test_paused_front_task_reports_its_own_partial_capture(tmp_path, current_samples): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') + try: + ready(host, clock) + assert host.start().success + session = host.execution.session + session._unit_index = next(i for i, unit in enumerate(session._units) + if unit.task_key == 'thumb_pitch_ip_front' and unit.direction == 'decreasing') + session.phase = Phase.PAUSED + unit = session.current_unit + host.execution.last_quality = SweepQuality(True, (), (), {'previous_yaw': { + 'valid_frames': 194, 'feedback_span': 1., 'feedback_bins': 132, 'maximum_bin_gap': 6}}) + rows = [] + for joint in ('rh_thumb_cmc_pitch', 'rh_thumb_ip'): + for index in range(current_samples): + rows.append({'kind': 'joint_sample', 'joint': joint, 'view': 'front', + 'task_name': unit.task_key, 'cycle': unit.cycle, 'direction': unit.direction, + 'attempt': 1, 'sample_phase': 'sweep', 'image_stamp_ns': index+1, + 'feedback_u8': 255-index}) + # Stale or different-direction observations cannot fill current bins. + rows += [dict(row, task_name='thumb_yaw_top') for row in rows] + rows += [dict(row, direction='increasing') for row in rows] + host._unit_rows = rows + status = host.snapshot().calibration + assert status.task.key == unit.task_key + assert status.acquisition.valid_samples == current_samples + assert status.acquisition.coverage_01 < .2 + assert status.acquisition.bins <= current_samples + assert host.execution.last_quality.metrics['previous_yaw']['valid_frames'] == 194 + finally: + host.close() + + +@pytest.mark.parametrize('phase', [Phase.PAUSED, Phase.ABORTED, Phase.FAILED]) +def test_interruption_phase_is_not_hidden_by_last_joint_zero_motion(tmp_path, phase): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') + try: + ready(host, clock) + assert host.start().success + host.execution.session.phase = phase + host._motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 40., + task_key='thumb_yaw_top', command_index=1, zero_joints=('rh_thumb_cmc_yaw',)) + snapshot = host.snapshot() + assert snapshot.calibration.phase == phase.value + assert snapshot.calibration.task.label == '采集关节零位：rh_thumb_cmc_yaw' + assert '阶段：采集并冻结关节零位' not in render_status_zh(snapshot.as_dict()) + finally: + host.close() + + +@pytest.mark.parametrize('diagnostic', [True, False]) +def test_diagnostic_status_separates_raw_corner_coverage_from_accepted_pose_coverage(tmp_path, diagnostic): + from test_diagnostic_quality import raw_frames + + host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb' if diagnostic else '') + try: + ready(host, clock) + assert host.start().success + host.execution.session.phase = Phase.SWEEP + unit = host.execution.session.current_unit + host._unit_rows = raw_frames(host.profile, unit) + snapshot = host.snapshot() + rendered = render_status_zh(snapshot.as_dict()) + if diagnostic: + assert snapshot.calibration.acquisition.valid_samples == 129 + assert snapshot.calibration.acquisition.coverage_01 == 1. + assert snapshot.diagnostic_capture['accepted_pose_quality']['passed'] is False + assert snapshot.diagnostic_capture['accuracy_validated'] is False + assert '原始观测 129；覆盖 100.0%' in rendered + assert '可信姿态样本 0；覆盖 0.0%（未进行精度验收）' in rendered + else: + assert snapshot.calibration.acquisition.valid_samples == 0 + assert snapshot.diagnostic_capture is None + assert '有效样本 0' in rendered and '原始观测' not in rendered + finally: + host.close() + + +def test_paused_retry_does_not_reuse_first_attempt_samples(tmp_path): + host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') + try: + ready(host, clock) + assert host.start().success + session = host.execution.session + unit = session.current_unit + session._retry_by_unit[(unit.task_key, unit.cycle, unit.direction)] = 1 + session.phase = Phase.PAUSED + host._unit_rows = [{'joint': 'rh_thumb_cmc_yaw', 'view': 'top', + 'task_name': unit.task_key, 'cycle': unit.cycle, 'direction': unit.direction, + 'attempt': 1, 'sample_phase': 'sweep', 'image_stamp_ns': i+1, + 'feedback_u8': i} for i in range(256)] + assert host.snapshot().calibration.acquisition.valid_samples == 0 + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_pnp.py b/src/linkerhand_calibration/test/test_pnp.py index cc6f166..e180b4b 100644 --- a/src/linkerhand_calibration/test/test_pnp.py +++ b/src/linkerhand_calibration/test/test_pnp.py @@ -103,7 +103,7 @@ def test_temporal_selection_breaks_near_reprojection_tie() -> None: assert selected == continuous -def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None: +def test_stateless_selector_requires_confirmation_for_large_branch_correction() -> None: stale_mirror = SquareTagPose( quaternion_xyzw=tuple( Rotation.from_euler("y", 55.0, degrees=True).as_quat() @@ -136,8 +136,8 @@ def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None: maximum_tag_tilt_rad=np.deg2rad(75.0), ) - assert reason == "" - assert selected == true_pose + assert reason == "pose_branch_unconfirmed" + assert selected is None @pytest.mark.parametrize("tie", [-0.01, 1.5, 2.0, float("nan"), float("inf")]) @@ -165,21 +165,19 @@ def test_tracker_recovers_after_timestamp_gap() -> None: # Keep both IPPE solutions near-tied so this exercises gap recovery, # rather than correction of a branch with clearly worse image fit. second_rotation = Rotation.from_euler("y", 2.0, degrees=True) - first, first_reason = tracker.estimate( - "t0", - _project(first_rotation, np.asarray([0.0, 0.0, 0.25])), - tag_size_m=0.01, - camera_matrix=_camera_matrix(), - stamp_ns=1_000_000_000, - ) + for index in range(5): + first, first_reason = tracker.estimate( + "t0", _project(first_rotation, np.asarray([0.0, 0.0, 0.25])), + tag_size_m=0.01, camera_matrix=_camera_matrix(), + stamp_ns=1_000_000_000 + index * 30_000_000) rejected, rejection_reason = tracker.estimate( "t0", _project(second_rotation, np.asarray([0.0, 0.0, 0.25])), tag_size_m=0.01, camera_matrix=_camera_matrix(), - stamp_ns=1_100_000_000, + stamp_ns=1_150_000_000, ) - recovered, recovered_reason = tracker.estimate( + ambiguous, ambiguous_reason = tracker.estimate( "t0", _project(second_rotation, np.asarray([0.0, 0.0, 0.25])), tag_size_m=0.01, @@ -191,6 +189,16 @@ def test_tracker_recovers_after_timestamp_gap() -> None: assert first_reason == "" assert rejected is None assert rejection_reason == "pose_jump" + assert ambiguous is None + assert ambiguous_reason == "pose_branch_unconfirmed" + # A real timestamp gap permits reacquisition, but cannot itself resolve + # ambiguous image evidence. A larger projected Tag supplies a distinct + # image-fit winner; independent frames must then confirm it again. + for index in range(5): + recovered, recovered_reason = tracker.estimate( + "t0", _project(second_rotation, np.asarray([0.0, 0.0, 0.25]), tag_size_m=0.05), + tag_size_m=0.05, camera_matrix=_camera_matrix(), + stamp_ns=1_800_000_000 + index * 30_000_000) assert recovered is not None assert recovered_reason == "" diff --git a/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py b/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py new file mode 100644 index 0000000..09bdc82 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py @@ -0,0 +1,309 @@ +"""Distinct image evidence must authorize planar branch changes before capture. + +The small real fixture records six O6 discontinuities, not physical ground +truth. Smooth or repeatedly preferred IPPE branches are not accuracy evidence. +""" + +from __future__ import annotations + +import json +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.pnp import SquareTagPose, SquareTagPoseTracker, solve_square_tag_ippe + + +REAL = json.loads((Path(__file__).parent / "fixtures" / + "o6_thumb_20260914_branch_events.json").read_text()) +START_NS = 1_000_000_000 +STEP_NS = 33_000_000 + + +def _distance(first, second): + return float(np.degrees((Rotation.from_quat(first).inv() * + Rotation.from_quat(second)).magnitude())) + + +def _pose(angle, error): + return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), + (0.0, 0.0, 0.5), float(error)) + + +@pytest.fixture +def source(monkeypatch): + """Control candidate evidence while exercising the real stateful tracker.""" + class Source: + candidates = [_pose(0, .05), _pose(60, .20)] + + def sample(self, tracker, stamp_ns, *, role="moving"): + return tracker.estimate(role, [(0, 0), (1, 0), (1, 1), (0, 1)], + tag_size_m=.016, camera_matrix=np.eye(3), stamp_ns=stamp_ns) + + instance = Source() + monkeypatch.setattr(tracking, "solve_square_tag_ippe", + lambda *args, **kwargs: list(instance.candidates)) + return instance + + +def _confirm(source, tracker, *, start=START_NS, role="moving"): + outcomes = [source.sample(tracker, start + i * STEP_NS, role=role) for i in range(5)] + assert all(pose is None and reason for pose, reason in outcomes[:-1]) + pose, reason = outcomes[-1] + assert pose is not None and reason == "" + return pose, start + 4 * STEP_NS + + +def _freeze(tracker, role="moving"): + revision = tracker.last_candidate_diagnostics_by_role[role]["branch_revision"] + assert tracker.freeze_roles({role: revision}) is True + return revision + + +def test_separated_initial_candidates_need_five_unique_images_and_elapsed_time(source): + tracker = SquareTagPoseTracker() + for i in range(8): + pose, reason = source.sample(tracker, START_NS + i * 1_000_000) + assert pose is None and reason + pose, reason = source.sample(tracker, START_NS + 100_000_000) + assert pose is not None and reason == "" + assert _distance(pose.quaternion_xyzw, _pose(0, .05).quaternion_xyzw) < 1e-9 + detail = tracker.last_candidate_diagnostics_by_role["moving"] + assert detail["observation_stamp_ns"] == START_NS + 100_000_000 + + +def test_duplicate_images_do_not_advance_confirmation(source): + tracker = SquareTagPoseTracker() + for _ in range(8): + pose, reason = source.sample(tracker, START_NS) + assert pose is None and reason + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_confirmation_count"] == 1 + for i in range(1, 4): + pose, reason = source.sample(tracker, START_NS + i * STEP_NS) + assert pose is None and reason + pose, reason = source.sample(tracker, START_NS + 4 * STEP_NS) + assert pose is not None and reason == "" + + +def test_near_equal_image_fit_never_accumulates_false_initial_confidence(source): + tracker = SquareTagPoseTracker() + source.candidates = [_pose(0, .05), _pose(60, .079)] + for i in range(20): + pose, reason = source.sample(tracker, START_NS + i * STEP_NS) + assert pose is None and reason + assert tracker.reprojection_tie_px == .03 + assert tracker.freeze_roles({"moving": 0}) is False + + +def test_confirmation_does_not_pool_alternating_branches_or_other_roles(source): + tracker = SquareTagPoseTracker() + for i in range(16): + source.candidates = ([_pose(0, .05), _pose(60, .20)] if (i // 2) % 2 == 0 + else [_pose(0, .20), _pose(60, .05)]) + pose, reason = source.sample(tracker, START_NS + i * STEP_NS) + assert pose is None and reason + source.candidates = [_pose(0, .05), _pose(60, .20)] + pose, reason = source.sample(tracker, START_NS + 16 * STEP_NS, role="another") + assert pose is None and reason + assert tracker.last_candidate_diagnostics_by_role["another"]["branch_confirmation_count"] == 1 + + +@pytest.mark.parametrize("candidates", [ + [_pose(0, .05)], [_pose(0, .05), _pose(.5, .06)], + [_pose(0, .05), _pose(60, 1.51)], +]) +def test_single_or_equivalent_solution_confirms_without_an_image_error_margin(source, candidates): + source.candidates = candidates + pose, _ = _confirm(source, SquareTagPoseTracker()) + assert pose is not None + + +def test_unfrozen_stale_branch_is_revised_without_emitting_the_switch_frame(source): + tracker = SquareTagPoseTracker() + old, stamp = _confirm(source, tracker) + old_revision = tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] + source.candidates = [_pose(0, .20), _pose(60, .05)] + for i in range(1, 5): + pose, reason = source.sample(tracker, stamp + i * STEP_NS) + assert pose is None and reason + pose, reason = source.sample(tracker, stamp + 5 * STEP_NS) + assert pose is None and reason == "pose_branch_corrected" + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == old_revision + 1 + assert tracker.branch_correction_counts["moving"] == 1 + pose, reason = source.sample(tracker, stamp + 6 * STEP_NS) + assert pose is not None and reason == "" + assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(60) + assert tracker.freeze_roles({"moving": old_revision}) is False + assert tracker.freeze_roles({"moving": old_revision + 1}) is True + + +def test_freezing_multiple_roles_is_atomic_and_requires_confirmed_revisions(source): + tracker = SquareTagPoseTracker() + _confirm(source, tracker, role="first") + _confirm(source, tracker, role="second") + revisions = {role: tracker.last_candidate_diagnostics_by_role[role]["branch_revision"] + for role in ("first", "second")} + assert tracker.freeze_roles({**revisions, "second": revisions["second"] + 1}) is False + for role in revisions: + source.sample(tracker, START_NS + 5 * STEP_NS, role=role) + assert tracker.last_candidate_diagnostics_by_role[role]["branch_frozen"] is False + assert tracker.freeze_roles(revisions) is True + + +def test_short_conflict_discards_images_then_original_evidence_can_resume(source): + tracker = SquareTagPoseTracker() + old, stamp = _confirm(source, tracker) + revision = _freeze(tracker) + source.candidates = [_pose(0, .20), _pose(60, .05)] + for i in (1, 2): + pose, reason = source.sample(tracker, stamp + i * STEP_NS) + assert pose is None and reason + source.candidates = [_pose(.2, .05), _pose(60, .20)] + pose, reason = source.sample(tracker, stamp + 3 * STEP_NS) + assert pose is not None and reason == "" + assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.2) + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision + assert tracker.branch_correction_counts.get("moving", 0) == 0 + + +def test_frozen_tracker_keeps_original_continuity_inside_the_unchanged_point03_tie(source): + tracker = SquareTagPoseTracker() + original, stamp = _confirm(source, tracker) + revision = _freeze(tracker) + source.candidates = [_pose(.2, .079), _pose(60, .05)] + pose, reason = source.sample(tracker, stamp + STEP_NS) + assert pose is not None and reason == "" + assert _distance(original.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.2) + assert tracker.reprojection_tie_px == .03 + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision + + +def test_observation_snapshot_matches_only_its_own_image(source): + tracker = SquareTagPoseTracker() + _, stamp = _confirm(source, tracker) + candidates, detail, corrections = tracker.observation_snapshot("moving", stamp) + assert len(candidates) == 2 + assert detail["observation_stamp_ns"] == stamp + assert corrections == 0 + source.sample(tracker, stamp + STEP_NS) + assert tracker.observation_snapshot("moving", stamp) is None + assert tracker.observation_snapshot("unknown", stamp + STEP_NS) is None + assert tracker.observation_snapshot("moving", stamp + STEP_NS) is not None + + +def test_persistent_ambiguity_keeps_the_frozen_path_and_can_recover_after_five_seconds(source): + tracker = SquareTagPoseTracker() + _, stamp = _confirm(source, tracker) + revision = _freeze(tracker) + source.candidates = [_pose(0, .20), _pose(60, .05)] + for i in range(1, 190): + pose, reason = source.sample(tracker, stamp + i * STEP_NS) + assert pose is None and reason == "pose_branch_ambiguous" + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision + source.candidates = [_pose(0, .05), _pose(60, .20)] + pose, reason = source.sample(tracker, stamp + 190 * STEP_NS) + assert pose is not None and reason == "" + assert not tracker._branches.states['moving'].conflict + assert tracker.branch_correction_counts.get('moving', 0) == 0 + + +@pytest.mark.parametrize("ambiguous_frames", [0, 12]) +def test_frozen_tracker_still_stops_when_no_candidate_continues_the_original_path(source, ambiguous_frames): + tracker = SquareTagPoseTracker() + _, stamp = _confirm(source, tracker) + revision = _freeze(tracker) + source.candidates = [_pose(0, .20), _pose(60, .05)] + for index in range(1, ambiguous_frames + 1): + assert source.sample(tracker, stamp + index*STEP_NS) == (None, "pose_branch_ambiguous") + stamp += ambiguous_frames * STEP_NS + source.candidates = [_pose(60, .05), _pose(70, .2)] + for index in range(1, 7): + pose, reason = source.sample(tracker, stamp + index*STEP_NS) + assert pose is None + assert reason == ('pose_branch_unconfirmed' if index < 5 else 'pose_branch_conflict') + source.candidates = [_pose(0, .05), _pose(60, .2)] + assert source.sample(tracker, stamp + 7*STEP_NS) == (None, 'pose_branch_conflict') + assert tracker._branches.states['moving'].revision == revision + + +def test_unfrozen_actual_gap_allows_new_confirmation(source): + tracker = SquareTagPoseTracker() + _, stamp = _confirm(source, tracker) + source.candidates = [_pose(0, .20), _pose(60, .05)] + pose, _ = _confirm(source, tracker, start=stamp + 6_000_000_000) + assert _distance(pose.quaternion_xyzw, _pose(60, .05).quaternion_xyzw) < 1e-9 + + +def test_frozen_actual_gap_requires_original_pose_before_reconfirmation(source): + tracker = SquareTagPoseTracker() + old, stamp = _confirm(source, tracker) + revision = _freeze(tracker) + source.candidates = [_pose(2, .05), _pose(60, .20)] + start = stamp + 6_000_000_000 + for i in range(6): + pose, reason = source.sample(tracker, start + i * STEP_NS) + assert pose is None and reason == "pose_branch_recovery_required" + source.candidates = [_pose(.5, .05), _pose(60, .20)] + pose, _ = _confirm(source, tracker, start=start + 6 * STEP_NS) + assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.5) + assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision + + +def _real_sample(tracker, role, frame): + return tracker.estimate(role, frame["corners_xy"], tag_size_m=REAL["tag_size_m"], + camera_matrix=REAL["camera_matrix"], stamp_ns=frame["stamp_ns"]) + + +@pytest.mark.parametrize("sequence", REAL["zero_sequences"], ids=lambda s: f"real_ID{s['tag_id']}") +def test_real_stationary_zero_frames_confirm_consistent_evidence_without_ground_truth(sequence): + tracker = SquareTagPoseTracker() + role = sequence["role"] + output = [_real_sample(tracker, role, frame) for frame in sequence["frames"]] + assert all(pose is None and reason for pose, reason in output[:4]) + assert all(pose is not None and reason == "" for pose, reason in output[4:]) + _freeze(tracker, role) + # Independent IPPE replay shows the recorded preference exceeds the existing + # 0.03 px tie tolerance; this is explicitly not a true-pose assertion. + for frame in sequence["frames"]: + candidates = solve_square_tag_ippe(frame["corners_xy"], + tag_size_m=REAL["tag_size_m"], camera_matrix=REAL["camera_matrix"]) + assert len(candidates) == 2 + errors = sorted(p.reprojection_error_px for p in candidates) + assert errors[1] - errors[0] > .03 + + +@pytest.mark.parametrize("event", REAL["jump_events"], + ids=lambda e: f"real_ID{e['tag_id']}_{e['event_stamp_ns']}") +def test_real_six_discontinuities_never_enter_a_frozen_observation_stream(event): + tracker = SquareTagPoseTracker() + role = event["role"] + warmup = event["warmup_frames"] + assert len({frame["stamp_ns"] for frame in warmup}) >= 5 + assert warmup[-1]["stamp_ns"] - warmup[0]["stamp_ns"] >= 100_000_000 + outputs = [_real_sample(tracker, role, frame) for frame in warmup] + previous, reason = outputs[-1] + assert previous is not None and reason == "" + revision = _freeze(tracker, role) + for frame in event["frames"]: + selected, reason = _real_sample(tracker, role, frame) + if selected is None: + assert reason + continue + assert reason == "" + assert _distance(previous.quaternion_xyzw, selected.quaternion_xyzw) <= 35 + previous = selected + detail = tracker.last_candidate_diagnostics_by_role[role] + assert detail["branch_revision"] == revision + assert detail["branch_frozen"] is True + assert tracker.branch_correction_counts.get(role, 0) == 0 + # The source really contains the claimed discontinuity; the regression + # cannot become vacuous by replacing the fixture with benign corners. + frames = event["frames"] + index = next(i for i, f in enumerate(frames) if f["stamp_ns"] == event["event_stamp_ns"]) + jump = _distance(frames[index - 1]["recorded_selected_quaternion_xyzw"], + frames[index]["recorded_selected_quaternion_xyzw"]) + assert jump == pytest.approx(event["recorded_jump_deg"], abs=1e-9) + assert jump > 35 diff --git a/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py b/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py new file mode 100644 index 0000000..dec45d9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py @@ -0,0 +1,93 @@ +"""PnP computation must not block freezing or commit across reset/image versions.""" + +from concurrent.futures import ThreadPoolExecutor +from threading import Event + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.core.geometry.pnp import SquareTagPose, SquareTagPoseTracker + + +def pose(angle, error): + return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), + (0., 0., .5), error) + + +def sample(tracker, stamp): + return tracker.estimate("tag", np.zeros((4, 2)), tag_size_m=.016, + camera_matrix=np.eye(3), stamp_ns=stamp) + + +def initialized(monkeypatch): + tracker = SquareTagPoseTracker() + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [pose(0, .05), pose(60, .2)]) + for i in range(5): + selected, _ = sample(tracker, 1_000_000_000 + i * 30_000_000) + assert selected is not None + return tracker + + +def test_freeze_during_inflight_pnp_applies_before_branch_selection(monkeypatch): + tracker = initialized(monkeypatch) + entered, release = Event(), Event() + + def solve(*args, **kwargs): + entered.set() + assert release.wait(5) + return [pose(0, .2), pose(60, .05)] + + monkeypatch.setattr(tracking, "solve_square_tag_ippe", solve) + with ThreadPoolExecutor(max_workers=2) as pool: + pending = pool.submit(sample, tracker, 1_150_000_000) + try: + assert entered.wait(5) + # This completes while the expensive solver is still blocked. + assert pool.submit(tracker.freeze_roles, {"tag": 1}).result(timeout=2) + finally: + release.set() + selected, reason = pending.result(timeout=2) + assert selected is None and reason == "pose_branch_ambiguous" + for i in range(1, 5): + selected, reason = sample(tracker, 1_150_000_000 + i * 30_000_000) + assert selected is None and reason == "pose_branch_ambiguous" + assert tracker.last_candidate_diagnostics_by_role["tag"]["branch_revision"] == 1 + + +@pytest.mark.parametrize("operation", ["reset", "newer_image"]) +def test_late_pnp_cannot_overwrite_newer_metadata_or_repopulate_reset(monkeypatch, operation): + tracker = initialized(monkeypatch) + entered, release = Event(), Event() + calls = 0 + + def solve(*args, **kwargs): + nonlocal calls + calls += 1 + if calls == 1: + entered.set() + assert release.wait(5) + return [pose(60, .05), pose(0, .2)] + return [pose(.1, .05), pose(60, .2)] + + monkeypatch.setattr(tracking, "solve_square_tag_ippe", solve) + with ThreadPoolExecutor(max_workers=1) as pool: + pending = pool.submit(sample, tracker, 1_150_000_000) + try: + assert entered.wait(5) + if operation == "reset": + tracker.reset() + else: + assert sample(tracker, 1_180_000_000)[0] is not None + finally: + release.set() + selected, reason = pending.result(timeout=2) + assert selected is None + if operation == "reset": + assert reason == "pose_observation_superseded" + assert not tracker.last_candidate_diagnostics_by_role + else: + assert reason == "pose_observation_not_new" + assert tracker.observation_snapshot("tag", 1_150_000_000) is None + assert tracker.observation_snapshot("tag", 1_180_000_000)[1]["branch_status"] == "tracking" diff --git a/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py b/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py new file mode 100644 index 0000000..4459931 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py @@ -0,0 +1,50 @@ +"""ROS and offline capture share typed tracking defaults and parameter units.""" + +from dataclasses import asdict +import math +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.core.geometry.pnp import DEFAULT_POSE_TRACKING_PARAMETERS +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime import parameters as runtime_parameters +from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, tracking_parameters + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def test_every_profile_uses_same_shared_confirmation_defaults(layout): + values = parameter_defaults(load_bundled_hand_profile(layout)) + resolved = tracking_parameters(values.__getitem__) + assert resolved == DEFAULT_POSE_TRACKING_PARAMETERS + assert values["pnp_branch_confirmation_frames"] == 5 + assert values["pnp_branch_confirmation_minimum_seconds"] == .1 + assert values["pnp_branch_equivalence_deg"] == 1. + assert resolved.reprojection_tie_px == .03 + assert resolved.maximum_pose_jump_rad == math.radians(35) + assert resolved.maximum_tag_tilt_rad == math.radians(75) + assert resolved.maximum_reprojection_error_px == 1.5 + + +def test_ros_converts_branch_angle_and_preserves_custom_confirmation_values(): + values = parameter_defaults(load_bundled_hand_profile("o6_right_8")) + values.update(pnp_branch_confirmation_frames=7, + pnp_branch_confirmation_minimum_seconds=.25, pnp_branch_equivalence_deg=.5) + resolved = tracking_parameters(values.__getitem__) + assert resolved.branch_confirmation_frames == 7 + assert resolved.branch_confirmation_minimum_seconds == .25 + assert resolved.branch_equivalence_rad == math.radians(.5) + + +def test_runtime_passes_all_tracking_fields_to_each_independent_view(monkeypatch): + values = parameter_defaults(load_bundled_hand_profile("o6_right_8")) + values.update(pnp_branch_confirmation_frames=8, + pnp_branch_confirmation_minimum_seconds=.2, pnp_branch_equivalence_deg=.8) + tracking = tracking_parameters(values.__getitem__) + monkeypatch.setattr(runtime_parameters, "SquareTagPoseTracker", + lambda **kwargs: SimpleNamespace(parameters=kwargs)) + trackers = runtime_parameters.RuntimeParameters.new_trackers( + SimpleNamespace(tracking=tracking), ("front", "side", "top")) + assert set(trackers) == {"front", "side", "top"} + assert len({id(value) for value in trackers.values()}) == 3 + assert all(value.parameters == asdict(tracking) for value in trackers.values()) diff --git a/src/linkerhand_calibration/test/test_pnp_runtime_policy.py b/src/linkerhand_calibration/test/test_pnp_runtime_policy.py index f720942..d065bee 100644 --- a/src/linkerhand_calibration/test/test_pnp_runtime_policy.py +++ b/src/linkerhand_calibration/test/test_pnp_runtime_policy.py @@ -31,19 +31,23 @@ def _check_branch_selection(tracker, monkeypatch): accurate = _pose(2, 0.05) continued = _pose(3, 0.11) frames = iter([ - (stale,), - (_pose(54, 0.24), accurate), + *[(stale,)] * 5, + *[(_pose(54, 0.24), accurate)] * 5, # A real near-tie still favors continuous motion over a mirror flip. (_pose(55, 0.10), continued), ]) monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **kw: next(frames)) - for index, expected in enumerate((stale, accurate, continued)): + for index in range(11): selected, reason = tracker.estimate( "moving", np.zeros((4, 2)), tag_size_m=0.016, camera_matrix=np.eye(3), stamp_ns=1_000_000_000 + index * 30_000_000, ) - assert reason == "" - assert selected == expected + if index in (4, 10): + assert reason == "" + assert selected == (stale if index == 4 else continued) + else: + assert selected is None + assert reason == ("pose_branch_corrected" if index == 9 else "pose_branch_unconfirmed") assert tracker.branch_correction_counts == {"moving": 1} diff --git a/src/linkerhand_calibration/test/test_pose_capture_evidence.py b/src/linkerhand_calibration/test/test_pose_capture_evidence.py new file mode 100644 index 0000000..ae965d3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pose_capture_evidence.py @@ -0,0 +1,125 @@ +"""Capture diagnostics preserve actual image evidence without changing poses.""" + +from dataclasses import replace +import json + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.domain.reference import build_joint_zero_reference +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.capture import CaptureTiming + +from test_observation_capture import frame, make_capture + + +def _lock(profile, lock, capture): + lock.begin_session() + lock.start_locking() + for stamp in range(1, 15): + capture.consume(frame(profile, stamp)) + + +def _sweep(profile): + return MotionCommand("sweep", tuple(profile.command.baseline_values), 1, + "thumb_pitch_dip_front", 0, 0, "decreasing") + + +def test_evidence_contains_this_images_corners_sizes_candidates_and_selected_pose(): + profile, _, capture = make_capture() + for stamp in range(1, 6): + capture.consume(frame(profile, stamp)) + observation = frame(profile, 20) + rows, movement = capture.consume(observation, _sweep(profile)) + assert movement is None and len(rows) == 2 + assert {row["kind"] for row in rows} == {"joint_sample"} + row = next(row for row in rows if row["joint"] == "rh_thumb_cmc_pitch") + evidence = row["pnp_observation_evidence"] + sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags} + for item in evidence.values(): + role = item["tag_role"] + assert item["pose_source"] == "current_image" + assert item["corners_xy"] == observation.corners[role].tolist() + assert item["tag_size_m"] == sizes[role] + candidates = capture.trackers["front"].last_candidates_by_role[role] + assert len(item["reprojection_valid_candidates"]) == len(candidates) > 0 + for recorded, actual in zip(item["reprojection_valid_candidates"], candidates): + assert recorded["quaternion_xyzw"] == list(actual.quaternion_xyzw) + assert recorded["translation_xyz_m"] == list(actual.translation_xyz_m) + assert recorded["reprojection_error_px"] == actual.reprojection_error_px + assert item["candidate_diagnostics"] == capture.trackers["front"].last_candidate_diagnostics_by_role[role] + # The image fixture places both Tags at the same orientation, with a + # known translation. Adding diagnostics must retain the actual solve. + parent = evidence["parent"]["selected_pose"] + child = evidence["child"]["selected_pose"] + rp = Rotation.from_quat(parent["quaternion_xyzw"]) + rc = Rotation.from_quat(child["quaternion_xyzw"]) + np.testing.assert_allclose((rp.inv()*rc).as_quat(), row["relative_quaternion_xyzw"], atol=1e-12) + np.testing.assert_allclose(parent["translation_xyz_m"], [0, 0, .45], atol=1e-8) + np.testing.assert_allclose(child["translation_xyz_m"], [.05, .03, .45], atol=1e-8) + frozen = json.dumps(evidence, sort_keys=True) + observation.corners["thumb_pitch"][:] = 0 + capture.trackers["front"].last_candidate_diagnostics_by_role["thumb_pitch"]["solved_candidate_count"] = 999 + capture.consume(frame(profile, 21, shift=1), _sweep(profile)) + assert json.dumps(evidence, sort_keys=True) == frozen + + +def test_zero_reference_ignores_additional_diagnostics_and_retains_original_pose(): + profile, lock, capture = make_capture() + _lock(profile, lock, capture) + 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 stamp in range(20, 30): + samples, movement = capture.consume(frame(profile, stamp, 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) + assert all(row["pnp_observation_evidence"]["child"]["tag_role"] == "thumb_pitch" for row in rows) + legacy = [{key: value for key, value in row.items() if key != "pnp_observation_evidence"} for row in rows] + assert build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2) == ( + build_joint_zero_reference(profile, name, legacy, session_epoch=1, motion_version=2)) + + +def test_missing_fixed_tag_uses_frozen_pose_but_never_copies_earlier_image_candidates(): + profile, lock, capture = make_capture() + _lock(profile, lock, capture) + assert capture.trackers["front"].last_candidates_by_role["front_base"] + rows, movement = capture.consume(frame(profile, 20, omit=("front_base",)), _sweep(profile)) + assert movement is None and len(rows) == 2 + row = next(row for row in rows if row["joint"] == "rh_thumb_cmc_pitch") + parent = row["pnp_observation_evidence"]["parent"] + assert parent["pose_source"] == "cached_fixed_reference" + assert parent["corners_xy"] is None + assert parent["reprojection_valid_candidates"] == [] + assert parent["candidate_diagnostics"] == {} + assert parent["selected_pose"]["quaternion_xyzw"] == list(capture.locked_poses["front"].quaternion_xyzw) + assert "image_relative_xy_px" not in row + assert row["pnp_observation_evidence"]["child"]["pose_source"] == "current_image" + + +def test_rejected_current_fixed_tag_corners_are_distinct_from_cached_pose(): + profile, lock, capture = make_capture() + _lock(profile, lock, capture) + observation = frame(profile, 20) + observation = replace(observation, corners={**observation.corners, "front_base": np.zeros((4, 2))}) + rows, movement = capture.consume(observation, _sweep(profile)) + assert movement is None + assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) + + +def test_unmatched_diagnostic_frame_preserves_timing_without_claiming_zero_skew(): + profile, lock, capture = make_capture() + _lock(profile, lock, capture) + observation = replace(frame(profile, 20), feedback=None, + timing=CaptureTiming(30, 35, 10, 18)) + rows, movement = capture.consume(observation, _sweep(profile), diagnostic=True) + assert movement is None + row = next(row for row in rows if row["kind"] == "diagnostic_observation_frame") + assert row["feedback_vector"] is None + assert row["state_image_sync_error_ms"] is None + assert row["timing"] == {"received_stamp_ns": 30, "history_snapshot_stamp_ns": 35, + "latest_feedback_stamp_ns": 10, "latest_command_stamp_ns": 18} + assert not any(row.get("kind") == "joint_sample" for row in rows) diff --git a/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py b/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py new file mode 100644 index 0000000..7ae59e3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py @@ -0,0 +1,112 @@ +"""Older pose and capture schedules remain readable without authorizing reuse.""" + +import json + +import pytest + +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.acquisition import load_capture +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine +from linkerhand_calibration.runtime.joint_resume import JointResume +from linkerhand_calibration.runtime.resume import discover_resume_candidate + + +def header(profile, policy, *, schedule="current"): + result = {"kind": "session_start", "profile_id": profile.key.profile_id, + "serial_number": "TEST", "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "protected_hashes": {"source": "a" * 64}} + if policy is not None: + result["pose_tracking_policy_version"] = policy + if schedule is not None: + result["capture_schedule_version"] = ( + CalibrationEngine(profile).capture_schedule_version if schedule == "current" else schedule) + return result + + +def journal(directory, profile, policy, *, schedule="current"): + directory.mkdir() + rows = [header(profile, policy, schedule=schedule), {"kind": "fixed_base_reference_locked"}, + {"kind": "scan_unit_complete", "passed": True}] + path = directory / "raw_samples.jsonl" + path.write_text("\n".join(json.dumps(row) for row in rows), encoding="utf-8") + return path, rows + + +@pytest.mark.parametrize("policy", [None, "", "legacy_single_frame", "confirmed_branch_v0", "confirmed_branch_v1", + "confirmed_branch_v5_image_motion"]) +def test_discovery_skips_old_pose_policy_but_preserves_offline_reading(tmp_path, policy): + profile = load_bundled_hand_profile("o6_right_8") + current = tmp_path / "01_current" + journal(current, profile, POSE_TRACKING_POLICY_VERSION) + old, rows = journal(tmp_path / "02_old", profile, policy) + before = old.read_bytes() + found = discover_resume_candidate(tmp_path, profile_id=profile.key.profile_id, + serial_number="TEST", protected_hashes={"source": "a" * 64}) + assert found == current + assert load_capture(old) == rows + assert old.read_bytes() == before + + +@pytest.mark.parametrize("schedule", [None, "unified_schedule_v1_sweep_then_steady"]) +def test_old_capture_schedule_is_readable_but_cannot_be_discovered_or_staged(tmp_path, monkeypatch, schedule): + from linkerhand_calibration.runtime import joint_resume + profile = load_bundled_hand_profile("o6_right_8") + current = tmp_path / "01_current" + journal(current, profile, POSE_TRACKING_POLICY_VERSION) + old, rows = journal(tmp_path / "02_old", profile, POSE_TRACKING_POLICY_VERSION, + schedule=schedule) + before = old.read_bytes() + engine = CalibrationEngine(profile) + assert not engine.resume_compatible(rows[0]) + assert discover_resume_candidate(tmp_path, profile_id=profile.key.profile_id, + serial_number="TEST", protected_hashes={"source": "a" * 64}) == current + monkeypatch.setattr(joint_resume, "read_joint_zero_references", + lambda *args: pytest.fail("old schedule must be rejected before importing zeros")) + resume = JointResume(profile) + with pytest.raises(ValueError, match="joint_resume_checkpoint_policy_incompatible"): + resume.stage(engine, rows, set()) + assert resume.references == {} and resume.rows == () + assert load_capture(old) == rows + assert old.read_bytes() == before + + +@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +@pytest.mark.parametrize("policy", [None, "old_unconfirmed_branch", "confirmed_branch_v5_image_motion", + POSE_TRACKING_POLICY_VERSION]) +def test_explicit_engine_resume_requires_current_pose_policy(layout, policy): + profile = load_bundled_hand_profile(layout) + assert CalibrationEngine(profile).resume_compatible(header(profile, policy)) == ( + policy == POSE_TRACKING_POLICY_VERSION) + + +@pytest.mark.parametrize("policy", [None, "old_unconfirmed_branch", "confirmed_branch_v5_image_motion"]) +def test_joint_staging_rejects_old_policy_before_reading_references(monkeypatch, policy): + from linkerhand_calibration.runtime import joint_resume + profile = load_bundled_hand_profile("o6_right_8") + monkeypatch.setattr(joint_resume, "read_joint_zero_references", + lambda *args: pytest.fail("read old zero references")) + resume = JointResume(profile) + with pytest.raises(ValueError, match="joint_resume_checkpoint_policy_incompatible"): + resume.stage(CalibrationEngine(profile), [header(profile, policy)], set()) + assert resume.references == {} + assert resume.rows == () + + +def test_joint_staging_accepts_current_policy_without_changing_it(monkeypatch): + from linkerhand_calibration.runtime import joint_resume + from linkerhand_calibration.runtime import motion_provenance + profile = load_bundled_hand_profile("o6_right_8") + rows = [header(profile, POSE_TRACKING_POLICY_VERSION)] + monkeypatch.setattr(joint_resume, "read_joint_zero_references", lambda *args: {}) + # This isolated test exercises policy dispatch only. Real reference and + # provenance acceptance is covered with complete evidence in dedicated tests. + validated = [] + monkeypatch.setattr(motion_provenance, "validate_motion_provenance", + lambda *args, **kwargs: validated.append((args, kwargs))) + resume = JointResume(profile) + resume.stage(CalibrationEngine(profile), rows, set()) + assert resume.rows == tuple(rows) + assert rows[0]["pose_tracking_policy_version"] == POSE_TRACKING_POLICY_VERSION + assert rows[0]["capture_schedule_version"] == CalibrationEngine(profile).capture_schedule_version + assert len(validated) == 1 and validated[0][1]["references"] == {} diff --git a/src/linkerhand_calibration/test/test_prepared_resume.py b/src/linkerhand_calibration/test/test_prepared_resume.py new file mode 100644 index 0000000..2089fde --- /dev/null +++ b/src/linkerhand_calibration/test/test_prepared_resume.py @@ -0,0 +1,125 @@ +"""Checkpoint decoding/geometry replay must finish before live feedback starts.""" + +from copy import deepcopy +import json + +import pytest + +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine +from linkerhand_calibration.runtime.joint_resume import JointResume +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready + + +@pytest.mark.parametrize("changed_reference", [False, True]) +@pytest.mark.parametrize("model,layout", [("o6", "o6_right_8"), ("l6", "l6_right_8"), + ("o12", "o12_right_16"), ("g20", "g20_right_19")]) +def test_resume_tick_uses_prepared_data_and_still_checks_current_reference( + tmp_path, monkeypatch, model, layout, changed_reference): + profile = load_bundled_hand_profile(layout) + fingerprint = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "protected_hashes": {"source": "a" * 64}, + "fixed_corners_by_view": {v: [[10, 10], [20, 10], [20, 20], [10, 20]] for v in profile.vision.view_names}, + "fixed_poses": {v: {"rotation_xyzw": [0, 0, 0, 1], "translation_xyz_m": [0, 0, .5]} + for v in profile.vision.view_names}, "moving_tag_poses": {}} + rows = [{"kind": "session_start", "serial_number": "OFFLINE_COORDINATOR", + "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, + "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version}, fingerprint] + path = tmp_path / "previous.jsonl" + path.write_text("\n".join(json.dumps(row) for row in rows)) + staged = [] + # Actual provenance and per-unit quality replay are covered by their own + # recorded-data tests; here assert that this expensive boundary is pre-live. + monkeypatch.setattr(JointResume, "stage", lambda self, *args: staged.append(self)) + host, clock = coordinator_fixture(tmp_path, model=model, resume_raw_samples_path=path) + try: + assert len(staged) == 1 and not host.started and not clock.positions + assert host.joint_resume is not staged[0] + path.unlink() # The immutable loaded snapshot is sufficient after startup. + monkeypatch.setattr(JointResume, "stage", lambda *args: pytest.fail("geometry replay in live callback")) + ready(host, clock) + assert host.start().success + clock.now += .1 + host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns()) + host._current_fingerprint = deepcopy(fingerprint) + if changed_reference: + view = profile.vision.view_names[0] + host._current_fingerprint["fixed_corners_by_view"][view][0][0] += 20 + host.execution.session.phase = Phase.RESUME_VERIFY + host.tick() + assert host.execution.session.phase == Phase.PREPARE + assert host.feedback_fresh() + assert (host.joint_resume is staged[0]) is not changed_reference + assert host._resumed_count == 0 # Each old zero still needs physical verification. + finally: + host.close() + + +def test_unreadable_checkpoint_is_rejected_without_delaying_live_motion(tmp_path): + host, clock = coordinator_fixture(tmp_path, resume_raw_samples_path=tmp_path / "missing.jsonl") + try: + assert host._resume_checkpoint is None and host._resume_checkpoint_error + ready(host, clock) + assert host.start().success + clock.now += .1 + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + host.execution.session.phase = Phase.RESUME_VERIFY + host.tick() + assert host.execution.session.phase == Phase.PREPARE + assert host._resumed_count == 0 and not host.joint_resume.references + finally: + host.close() + + +@pytest.mark.parametrize("abort_during_import", [False, True]) +def test_large_task_import_yields_to_feedback_and_commits_only_after_fsync( + tmp_path, monkeypatch, abort_during_import): + from linkerhand_calibration.runtime import coordinator + + host, clock = coordinator_fixture(tmp_path) + try: + ready(host, clock) + assert host.start().success + clock.now += .1 + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + host.execution.session.phase = Phase.PREPARE + unit = host.execution.session.current_unit + key = (unit.task_key, unit.cycle, unit.direction) + rows = tuple({"kind": "import_test", "index": i} for i in range(257)) + monkeypatch.setattr(host.joint_resume, "take_task", lambda task: ((key,), rows)) + writes, events = [], [] + original = coordinator.append_jsonl_many + def write(path, batch, *, durable=True): + assert not durable and len(batch) <= 64 + # The whole transfer would exceed the one-second feedback gate; + # each bounded callback permits another real feedback reception. + clock.now += len(batch) * .01 + writes.extend(batch) + original(path, batch, durable=durable) + monkeypatch.setattr(coordinator, "append_jsonl_many", write) + original_sync = coordinator.sync_jsonl + def sync(path): + assert host._resumed_count == 0 + original_sync(path) + events.append("durable") + monkeypatch.setattr(coordinator, "sync_jsonl", sync) + host.tick() + assert host._resumed_count == 0 and len(writes) == 64 + if abort_during_import: + assert host.abort().success + host.tick() + assert host.state == "ABORTED" and host._resumed_count == 0 + assert len(writes) == 64 + else: + while host._resume_import is not None: + assert host.feedback_fresh() + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + host.tick() + assert writes == list(rows) and events == ["durable"] + assert host._resumed_count == 1 and host.state != "PAUSED" + finally: + monkeypatch.undo() + host.close() diff --git a/src/linkerhand_calibration/test/test_process_finalizer.py b/src/linkerhand_calibration/test/test_process_finalizer.py new file mode 100644 index 0000000..7dd07b3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_process_finalizer.py @@ -0,0 +1,138 @@ +import json +from concurrent.futures import ThreadPoolExecutor +from pathlib import Path +from threading import Event +import time +from functools import partial +import hashlib +import tracemalloc + +import pytest + +from linkerhand_calibration.runtime.artifacts.process_finalizer import ( + isolated_finalize_profile_session, read_journal_prefix, +) + + +def sample_finalizer(*, session_dir, records, publish, phase_changed): + assert publish is False + for phase in ("fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"): + phase_changed(phase) + return tuple(row["value"] for row in records) + + +def waiting_finalizer(*, session_dir, records, publish, phase_changed): + phase_changed("entered") + time.sleep(30) + Path(session_dir, "unexpected_completion").touch() + + +def test_bounded_journal_and_process_preserve_data_and_event_order(tmp_path): + path = tmp_path / "raw.jsonl" + prefix = b'{"value": 1}\n{"value": 2}\n' + path.write_bytes(prefix + b'{"value": 999}\n') + events = [] + result = isolated_finalize_profile_session(journal_path=path, journal_size=len(prefix), + records=({"value": -1},), phase_changed=events.append, cancelled=lambda: False, + finalizer=sample_finalizer, session_dir=tmp_path) + assert result == (1, 2) + assert events == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] + timing = json.loads((tmp_path/"finalization_timing.json").read_text()) + assert timing["backend"] == "isolated_process" + assert timing["journal_prefix_bytes"] == len(prefix) + with pytest.raises(ValueError, match="incomplete"): + read_journal_prefix(path, len(prefix)-1) + with pytest.raises(ValueError, match="cannot_publish"): + isolated_finalize_profile_session(journal_path=path, journal_size=len(prefix), + records=(), phase_changed=events.append, cancelled=lambda: False, publish=True) + + +def test_cancel_terminates_child_without_waiting_for_fit_or_publication(tmp_path): + path = tmp_path/"raw.jsonl" + path.write_bytes(b'{"value": 1}\n') + entered, cancelled = Event(), Event() + with ThreadPoolExecutor(max_workers=1) as executor: + future = executor.submit(isolated_finalize_profile_session, journal_path=path, + journal_size=path.stat().st_size, records=(), phase_changed=lambda _: entered.set(), + cancelled=cancelled.is_set, finalizer=waiting_finalizer, session_dir=tmp_path) + assert entered.wait(10) + cancelled.set() + with pytest.raises(ValueError, match="operator_abort"): + future.result(timeout=3) + assert not (tmp_path/"unexpected_completion").exists() + + +def test_child_invalid_input_is_reported_without_a_result(tmp_path): + path = tmp_path/"raw.jsonl" + path.write_bytes(b'{"value": 1}') + with pytest.raises(RuntimeError, match="incomplete_finalization_journal_prefix"): + isolated_finalize_profile_session(journal_path=path, journal_size=path.stat().st_size, + records=(), phase_changed=lambda _: None, cancelled=lambda: False, + finalizer=sample_finalizer, session_dir=tmp_path) + + +def test_large_journal_has_bounded_raw_buffer_memory(tmp_path): + """Large raw evidence must not be duplicated when the fit process starts.""" + path = tmp_path / "large.jsonl" + line = b'{"value": 1}' + b" " * (256 * 1024) + b"\n" + with path.open("wb") as stream: + for _ in range(64): + stream.write(line) + tracemalloc.start() + try: + rows = read_journal_prefix(path, path.stat().st_size) + _, peak = tracemalloc.get_traced_memory() + finally: + tracemalloc.stop() + assert rows == ({"value": 1},) * 64 + assert peak < 4 * 1024 * 1024 + + +@pytest.mark.parametrize("data,size,error", [ + (b'{"value": 1}\n', 100, "incomplete"), + (b'{"value": 1}\n[]\n', None, "invalid_finalization_journal_rows"), +]) +def test_journal_rejects_short_file_or_nonobject_record(tmp_path, data, size, error): + path = tmp_path / "invalid.jsonl" + path.write_bytes(data) + with pytest.raises(ValueError, match=error): + read_journal_prefix(path, len(data) if size is None else size) + + +def skip_standard_loader(_path): + pass + + +def test_real_finalizer_stages_in_child_and_only_controller_publishes(tmp_path): + from measured_capture_fixture import measured_capture + from linkerhand_calibration.runtime.artifacts.controller import FinalizationController, FinalizationInputs + from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session + from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper + profile, source, records, _ = measured_capture(tmp_path, layout="o6_right_8") + session = tmp_path/"session" + session.mkdir() + raw = session/"raw_samples.jsonl" + raw.write_text("".join(json.dumps(row)+"\n" for row in records)) + hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} + hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() + controller = FinalizationController(profile, session, isolated=True, + finalizer=partial(finalize_profile_session, standard_loader=skip_standard_loader)) + phases, authorizations = [], [] + def authorize(): + assert phases == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] + assert not (tmp_path/profile.artifacts.publication_pointer).exists() + authorizations.append(True) + try: + controller.start(FinalizationInputs(session, "SYNTHETIC_PROCESS_ONLY", source, hashes, + tuple(records), journal_path=raw, journal_size=raw.stat().st_size)) + deadline = time.monotonic()+60 + result = None + while result is None and time.monotonic() < deadline: + result = controller.poll(phases.append, authorize) + time.sleep(.01) + assert result is not None + assert authorizations == [True] + assert controller.poll(phases.append, authorize) is None + load_unified_mapper(session/"release_manifest.json") + finally: + controller.close() diff --git a/src/linkerhand_calibration/test/test_production_image_motion.py b/src/linkerhand_calibration/test/test_production_image_motion.py new file mode 100644 index 0000000..71ee8cb --- /dev/null +++ b/src/linkerhand_calibration/test/test_production_image_motion.py @@ -0,0 +1,194 @@ +"""Production image models preserve independent images and frozen geometry.""" +from dataclasses import asdict, replace +import json +import gzip +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( + resolve_image_motion, select_image_motion_frame, image_motion_model_from_dict, +) +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation +from test_motion_image_diagnostics import frames, RELATIONS + + +def recorded_image_frames(rows): + from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates + from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation + from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + + source = [] + for raw in rows: + roles = tuple(RoleCandidates(item["role"], tuple(SquareTagPose( + tuple(p["quaternion_xyzw"]), tuple(p["translation_xyz_m"]), p["reprojection_error_px"]) + for p in item["candidates"]), item["frozen"]) for item in raw["evidence"]["roles"]) + source.append(ImageMotionFrame(MotionEvidenceFrame(raw["evidence"]["stamp_ns"], roles), + tuple(tuple(v) for v in raw["camera_matrix"]), tuple(ImageRoleObservation( + o["role"], o["tag_size_m"], tuple(tuple(p) for p in o["corners_xy"])) for o in raw["observations"]))) + return tuple(source) + + +@pytest.fixture(scope="module") +def resolved(): + source = frames(depth_error=.008) + result = resolve_image_motion(source, RELATIONS) + assert result.resolved, result + return source, result + + +def test_noisy_pnp_depth_is_replaced_by_current_image_geometry(resolved): + source, result = resolved + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + clean = frames() + for frame, reference in zip(source[::5], clean[::5]): + selected = select_image_motion_frame(result.model, frame) + assert selected.resolved, selected + estimate = dict((p.role, p.pose) for p in selected.selections)["moving"] + actual = reference.evidence.roles[1].candidates[0] + assert np.linalg.norm(np.subtract(estimate.translation_xyz_m, actual.translation_xyz_m)) < 1e-6 + assert (Rotation.from_quat(estimate.quaternion_xyzw).inv() + * Rotation.from_quat(actual.quaternion_xyzw)).magnitude() < 1e-5 + serialized = asdict(result.model) + json.dumps(serialized, allow_nan=False) + assert image_motion_model_from_dict(serialized) == result.model + + +def test_frozen_geometry_does_not_absorb_tag_slide(resolved): + source, result = resolved + before = asdict(result.model) + frame = source[-1] + shifted = replace(frame.observations[1], corners_xy=tuple((x + 12, y) for x, y in frame.observations[1].corners_xy)) + check = select_image_motion_frame(result.model, replace(frame, observations=(frame.observations[0], shifted))) + assert not check.resolved + assert asdict(result.model) == before + + +def test_current_roots_camera_and_source_identity_are_required(resolved): + source, result = resolved + frame = source[-1] + invalid = replace(frame, camera_matrix=((1700., 0., 640.), *frame.camera_matrix[1:])) + assert not select_image_motion_frame(result.model, invalid).resolved + unfrozen = replace(frame.evidence.roles[0], frozen=False) + invalid = replace(frame, evidence=replace(frame.evidence, roles=(unfrozen, *frame.evidence.roles[1:]))) + assert not select_image_motion_frame(result.model, invalid).resolved + changed = asdict(result.model) + changed["validation_stamps_ns"] = changed["training_stamps_ns"] + with pytest.raises(ValueError, match="source_overlap"): + image_motion_model_from_dict(changed) + changed = asdict(result.model) + changed["maximum_reprojection_error_px"] = 5. + with pytest.raises(ValueError, match="quality_policy"): + image_motion_model_from_dict(changed) + + +def test_no_rotation_cannot_authorize_model(): + source = frames() + still = tuple(replace(source[0], evidence=replace(source[0].evidence, + stamp_ns=f.evidence.stamp_ns)) for f in source) + result = resolve_image_motion(still, RELATIONS) + assert not result.resolved + assert result.model is None + + +def test_alternatives_within_image_resolution_are_not_ranked_as_truth(): + source = frames(alternatives=True) + result = resolve_image_motion(source, RELATIONS) + assert len(result.hypotheses) == 2 + # A near-frontal noiseless plane still admits distinct shallow-tilt models + # inside the public pixel tolerance. A lower score alone cannot resolve it. + assert not result.resolved and result.reason == "image_motion_families_not_distinguishable" + assert result.model is None + + +def test_serial_graph_and_previously_frozen_parent_share_same_solver(): + source = frames(chain=True, depth_error=.001) + chain = (*RELATIONS, MotionRelation("distal", "moving", "terminal")) + result = resolve_image_motion(source, tuple(reversed(chain))) + assert result.resolved, result + final = select_image_motion_frame(result.model, source[-1]) + assert final.resolved and len(final.angles_rad) == 2 + # Roots may themselves move; use each image's already selected parent. + clean = frames(chain=True) + source = tuple(replace(f, evidence=replace(f.evidence, roles=tuple( + replace(r, frozen=True) if r.role == "moving" else r for r in f.evidence.roles))) for f in clean) + distal = resolve_image_motion(source, (chain[1],)) + assert distal.resolved, distal + + +def test_motion_stratified_evidence_requires_mean_and_rank_improvement(): + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import _comparison + angles = np.linspace(0, 1.2, 72)[:, None] + baseline = np.full(72, .3) + assert _comparison(baseline, baseline + .02, angles, .03) == 1. + assert _comparison(baseline, baseline + .2, angles, .03) < .001 + alternative = baseline.copy() + alternative[-2:] += 20 + assert _comparison(baseline, alternative, angles, .03) > .01 + + +def test_recorded_preparation_only_freeze_predicts_both_later_directions(): + fixture = Path(__file__).parent / "fixtures" / "o6_image_motion_200044.json.gz" + payload = json.loads(gzip.decompress(fixture.read_bytes())) + assert payload["source_sha256"] == "52f9ed7510fc58a4271532a2dc13a678f2e4aa2f1dc3153bf749fcf377cb23c5" + models, validated = {}, [] + for stage in payload["stages"]: + source = recorded_image_frames(stage["frames"]) + metadata = stage["metadata"] + if metadata["sample_phase"] == "zero_approach": + result = resolve_image_motion(source, tuple(MotionRelation(**r) for r in stage["relations"])) + assert result.resolved, (metadata, result) + models[metadata["view"]] = result.model + else: + assert metadata["cycle"] == 0 + model = models[metadata["view"]] + frozen = asdict(model) + assert set(f.evidence.stamp_ns for f in source).isdisjoint(model.source_stamps_ns) + for frame in source: + check = select_image_motion_frame(model, frame) + assert check.resolved, (metadata, check) + assert max(q.rms_px for q in check.quality) < .5 + assert asdict(model) == frozen + validated.append((metadata["view"], metadata["direction"])) + assert set(validated) == {(view, direction) for view in ("front", "top") + for direction in ("increasing", "decreasing")} + + +def test_recorded_pinky_alternatives_converge_within_original_budget(): + """The side-view preparation previously exhausted 150 steps in two families.""" + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters + + fixture = Path(__file__).parent / "fixtures" / "o6_pinky_preparation_144514.json.gz" + payload = json.loads(gzip.decompress(fixture.read_bytes())) + assert payload["source_sha256"] == "77d3337dfd5384409fcf4352e76c925b48938912b4d47c5341cc2c92d8cef332" + source = recorded_image_frames(payload["frames"]) + assert len(source) == 120 + limits = ImageMotionParameters() + assert limits.maximum_solver_evaluations == payload["recorded_exhausted_solver_evaluations"] == 150 + result = resolve_image_motion(source, tuple(MotionRelation(**r) for r in payload["relations"])) + assert result.resolved, result + assert len(result.hypotheses) == 4 + for hypothesis in result.hypotheses: + assert hypothesis.converged and hypothesis.training_solver_status > 0 + assert 0 < hypothesis.training_solver_evaluations <= 150 + assert len(result.model.training_stamps_ns) == len(result.model.validation_stamps_ns) == 60 + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + assert result.model.maximum_reprojection_error_px == 1.5 + + +def test_hinge_pivot_parameterization_is_continuous_across_axis_helper_boundary(): + from linkerhand_calibration.core.geometry.tag_pose.motion_image_solver import ImageHingeBundle + source = frames() + poses = {role.role: tuple(frame.evidence.roles[index].candidates[0] for frame in source) + for index, role in enumerate(source[0].evidence.roles)} + bundle = ImageHingeBundle(source, RELATIONS, poses) + first, second = bundle.initial.copy(), bundle.initial.copy() + first[5:7] = second[5:7] = [.02, .01] + first[0], second[0] = np.arcsin(.9) - 1e-8, np.arcsin(.9) + 1e-8 + axis_a, _, pivot_a, _ = bundle.geometry_values(first, 0) + axis_b, _, pivot_b, _ = bundle.geometry_values(second, 0) + assert np.linalg.norm(pivot_b - pivot_a) < 1e-8 + assert abs(axis_a @ pivot_a) < 1e-12 + assert abs(axis_b @ pivot_b) < 1e-12 diff --git a/src/linkerhand_calibration/test/test_profile_finalization.py b/src/linkerhand_calibration/test/test_profile_finalization.py index 311ebf1..c8e3345 100644 --- a/src/linkerhand_calibration/test/test_profile_finalization.py +++ b/src/linkerhand_calibration/test/test_profile_finalization.py @@ -32,9 +32,18 @@ def rigid(rpy, xyz): def virtual_capture(tmp_path, layout, *, include_baseline=False, profile_transform=None): base_layout = "l6_right_8" if layout == "virtual_reordered" else layout profile = load_bundled_hand_profile(base_layout) + # This synthetic fixture generates the original interleaved partitions. + profile = replace(profile, acquisition=replace(profile.acquisition, + command_capture_mode="interleaved", steady_extra_training_nodes={})) + # This independent FK sensor fixture supplies measured 3D poses, not + # corner images or IPPE candidates. Its synthetic observation profile + # therefore has no image-branch reconstruction stage. + profile = replace(profile, measurement=replace(profile.measurement, + candidate_selection_tasks=frozenset())) # This fixture preserves the legacy v2 acceptance contract. New measured # behavior is exercised separately using include_baseline and real v3 refs. - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2)) + profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, + directional_command_joints=profile.artifacts.directional_command_joints if include_baseline else frozenset())) if include_baseline: profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=3)) if profile_transform is not None: @@ -80,6 +89,10 @@ def virtual_capture(tmp_path, layout, *, include_baseline=False, profile_transfo "middle_pip": "middle_pip", "middle_dip": "middle_dip", "index_pip": "index_pip", "index_dip": "index_dip"} native_tag_joint.update(thumb_ip="thumb_ip", ring_roll="ring_mcp_roll", pinky_roll="pinky_mcp_roll", ring_pip="ring_pip", ring_dip="ring_dip") + if base_layout == "o6_right_8": + # Confirmed physical layout: top ID7 and front ID2 share the distal + # segment, even though only yaw is swept while observing ID7. + native_tag_joint["thumb_yaw"] = "thumb_ip" direct = profile.zero.direct_zero_joints offsets = {name: (0.04 if name.endswith(("roll", "yaw")) else -0.02 if "thumb" in name else 0.02) for name in direct} diff --git a/src/linkerhand_calibration/test/test_profile_loader.py b/src/linkerhand_calibration/test/test_profile_loader.py index 7a5951e..d357e87 100644 --- a/src/linkerhand_calibration/test/test_profile_loader.py +++ b/src/linkerhand_calibration/test/test_profile_loader.py @@ -78,7 +78,7 @@ artifacts: corrected_urdf_filename: corrected.urdf protected_input_fields: [source_urdf_sha256] publish_corrected_urdf: true -acquisition: {policy_version: unified_engine_v5_joint_zero} +acquisition: {policy_version: unified_engine_v8_all_view_images} urdf: authorized_fields: finger_joint: [origin.rpy, limit.lower, limit.upper] diff --git a/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py b/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py new file mode 100644 index 0000000..0cf42e6 --- /dev/null +++ b/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py @@ -0,0 +1,147 @@ +"""Real capture segments test evidence, never a fabricated complete hand run. + +175815 has old sweep images but no new pre-zero motion record. 192637 has the +actual pre-zero motion but stopped before its IP sweeps. They are replayed as +separate sessions; neither is promoted to a fourth-round accuracy certificate. +""" + +from copy import deepcopy +import gzip +import hashlib +import json +from pathlib import Path + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.pnp import solve_square_tag_ippe +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization +from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture +from linkerhand_calibration.runtime.diagnostic_quality import evaluate_diagnostic_unit +from linkerhand_calibration.runtime.diagnostic_report import motion_initialization_summary +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 + + +SOURCE_HASHES = { + "175815": "8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d", + "192637": "757ba1d7cccae4bdb93553b2735e94862bc74f57f153198e1a9d3eff76d45c8d", +} + + +def recorded_fixture(stamp): + path = Path(__file__).with_name("fixtures") / f"o6_front_20260914_{stamp}_raw_replay.json.gz" + value = json.loads(gzip.decompress(path.read_bytes())) + assert value["source_raw_sha256"] == SOURCE_HASHES[stamp] + assert value["publication_allowed"] is value["is_accuracy_certificate"] is False + return value + + +def replay_actual_front_initialization(): + fixture = recorded_fixture("192637") + profile = load_bundled_hand_profile("o6_right_8") + recorded = fixture["recorded_initialization"] + joints = tuple(recorded["zero_joints"]) + task = next(task for task in profile.motion.tasks if task.key == recorded["task_name"]) + target = profile.motion.joint_zero_references[joints[0]].command + common = dict(task_key=task.key, command_index=task.command_index) + # Reproduce this archived v4 decision with its original 3D candidate + # policy. The new image solver has separate original-corner regressions; + # it cannot change the meaning of this historical unresolved session. + owner = BranchInitialization(profile, image_geometry=False) + owner.begin(MotionCommand("zero_approach", target, 40., reference_joints=joints, **common), + session_epoch=recorded["session_epoch"], motion_version=recorded["motion_version"]) + for row in fixture["records"]: + owner.observe(row) + zero = MotionCommand("joint_zero", target, 40., zero_joints=joints, **common) + owner.begin(zero, session_epoch=recorded["session_epoch"], motion_version=recorded["motion_version"]+1) + request = owner.request("front", zero, session_epoch=recorded["session_epoch"], + motion_version=recorded["motion_version"]+1) + result = solve_initialization(request) + model, report = owner.accept(request, result) + return fixture, owner, zero, model, report + + +def test_recorded_pre_zero_frames_reach_runtime_decision_without_forged_continuation(): + fixture, owner, zero, model, report = replay_actual_front_initialization() + recorded = fixture["recorded_initialization"] + assert len(fixture["records"]) == 120 + assert all(row["sample_phase"] == "zero_approach" for row in fixture["records"]) + assert report["source_image_stamps"] == recorded["source_image_stamps"] + assert report["source_frame_hashes"] == recorded["source_frame_hashes"] + for row in fixture["records"]: + assert source_frame_sha256(row) == report["source_frame_hashes"][str(row["image_stamp_ns"])] + # This is the actual unresolved runtime evidence, not a test claiming + # that rejecting it repaired physical accuracy or completed its IP sweep. + assert model is None and not owner.ready(zero) + assert report["status"] == recorded["status"] == "unresolved" + assert report["reason"] == recorded["reason"] == "motion_geometry_unresolved" + assert not report["evidence_ids"] and not report["constraints"] + for actual, baseline in zip(report["hypotheses"], recorded["hypotheses"], strict=True): + assert actual["accepted"] == baseline["accepted"] + for current, previous in zip(actual["relations"], baseline["relations"], strict=True): + assert current["reason"] == previous["reason"] + for key, value in previous.items(): + if isinstance(value, (int, float)): + assert current[key] == pytest.approx(value, rel=1e-9, abs=1e-10) + + +def test_report_identifies_the_ip_relation_without_promoting_local_pitch_fit(): + fixture = recorded_fixture("192637") + summaries = motion_initialization_summary([fixture["recorded_initialization"]]) + report, = summaries + assert report["status"] == "unresolved" and report["source_image_count"] == 120 + pitch = report["joint_hypotheses"]["rh_thumb_cmc_pitch"] + ip = report["joint_hypotheses"]["rh_thumb_ip"] + assert any(not item["reason"] for item in pitch) + assert not any(item["chain_accepted"] for item in (*pitch, *ip)) + supported_parent = [item for item in ip if item["other_relations_passed"]] + assert len(supported_parent) == 2 + assert all(item["reason"] == "motion_geometry_residual" for item in supported_parent) + assert min(item["pivot_rms_m"] for item in supported_parent) == pytest.approx(.005102, abs=1e-6) + + +def test_legacy_real_sweep_corners_replay_without_becoming_new_zero_or_accepted_angles(): + fixture = recorded_fixture("175815") + records = fixture["records"] + before = deepcopy(records) + profile = load_bundled_hand_profile("o6_right_8") + for direction, count in (("decreasing", 198), ("increasing", 197)): + rows = [row for row in records if row["direction"] == direction] + assert len(rows) == count + for row in rows: + for role in ("thumb_pitch", "thumb_ip"): + tag = row["tags"][role] + candidates = solve_square_tag_ippe(tag["corners_xy"], + tag_size_m=tag["tag_size_m"], camera_matrix=row["camera_matrix"]) + assert candidates + for recorded in tag["reprojection_valid_candidates"]: + assert any(np.allclose(candidate.translation_xyz_m, + recorded["translation_xyz_m"], rtol=1e-10, atol=1e-12) + for candidate in candidates) + unit = next(unit for unit in CalibrationEngine(profile).scan_units() + if unit.task_key == "thumb_pitch_ip_front" and unit.cycle == 0 and unit.direction == direction) + quality = evaluate_diagnostic_unit(profile, unit, 1, rows) + assert quality.observations.passed, quality.observations.failures + assert not quality.accepted_poses.passed + assert all(item["valid_frames"] == 0 for item in quality.accepted_poses.metrics.values()) + with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"): + reject_diagnostic_capture([fixture["session_start"], *records]) + assert records == before + assert not any(row.get("kind") in {"joint_zero_reference", "joint_sample"} for row in records) + + +@pytest.mark.parametrize("stamp,expected", SOURCE_HASHES.items()) +def test_fixture_sources_remain_bound_when_full_local_journals_are_available(stamp, expected): + workspace = Path(__file__).resolve().parents[3] + path = workspace / "calibration_output/O6_RIGHT_001" / f"20260914_{stamp}" / "raw_samples.jsonl" + if not path.exists(): + pytest.skip("完整本地实测日志不可用；已有证据切片的回放另行执行") + assert hashlib.sha256(path.read_bytes()).hexdigest() == expected + fixture = recorded_fixture(stamp) + recorded = {(row.get("kind"), row.get("view"), row.get("image_stamp_ns")): row + for row in (json.loads(line) for line in path.read_text().splitlines())} + assert all(recorded[row["kind"], row["view"], row["image_stamp_ns"]] == row + for row in fixture["records"]) diff --git a/src/linkerhand_calibration/test/test_recorded_side_preparation.py b/src/linkerhand_calibration/test/test_recorded_side_preparation.py new file mode 100644 index 0000000..c714d6d --- /dev/null +++ b/src/linkerhand_calibration/test/test_recorded_side_preparation.py @@ -0,0 +1,48 @@ +"""Use the available preparation arc without reusing validation images.""" + +import gzip +import json +from pathlib import Path +from types import SimpleNamespace + +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( + ImageMotionParameters, resolve_image_motion, +) + + +def test_recorded_side_preserves_holdout_and_uses_source_geometry(): + path = Path(__file__).parent / "fixtures/o6_side_preparation_132102.json.gz" + payload = json.loads(gzip.decompress(path.read_bytes())) + rows = payload["records"] + config = load_product_config(Path(__file__).resolve().parents[1]/"config/o6_right_product.yaml", + check_can=False) + profile = config.calibration_contract.typed_profile + first = rows[0] + owner = BranchInitialization(profile, source_urdf=config.source_urdf) + motion = SimpleNamespace(phase="zero_approach", task_key=first["task_name"], + reference_joints=tuple(first["zero_joints"])) + version, epoch = first["motion_version"], first["session_epoch"] + owner.begin(motion, session_epoch=epoch, motion_version=version) + for row in rows: + owner.observe(row) + motion.phase, motion.zero_joints = "joint_zero", motion.reference_joints + request = owner.request("side", motion, session_epoch=epoch, motion_version=version + 1) + assert len(request.image_frames) == 121 + old = resolve_image_motion(request.image_frames, request.relations, + parameters=ImageMotionParameters(maximum_training_frames=36)) + assert not old.resolved and old.reason == "image_motion_families_not_distinguishable" + # Current production uses the same source geometry as the final URDF. + # Adding frames to the old free-geometry fit is not sufficient evidence + # that its competing three-dimensional reconstructions are distinguishable. + assert request.geometry_constraints and request.source_urdf_sha256 + result = solve_initialization(request) + assert result.resolved, result.reason + assert result.model.source_urdf_sha256 == request.source_urdf_sha256 + assert len(result.model.training_stamps_ns) == 61 + assert len(result.model.validation_stamps_ns) == 60 + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + probabilities = [p for h in result.hypotheses for _, p in h.comparison_p_values] + assert len(probabilities) == 3 + assert max(probabilities) <= .01 / 3 diff --git a/src/linkerhand_calibration/test/test_reference_waypoints.py b/src/linkerhand_calibration/test/test_reference_waypoints.py index 48307a2..445a149 100644 --- a/src/linkerhand_calibration/test/test_reference_waypoints.py +++ b/src/linkerhand_calibration/test/test_reference_waypoints.py @@ -26,7 +26,8 @@ def test_reference_waypoint_cannot_drive_disabled_channel(): def test_reference_waypoints_share_executor_and_return_before_resume(tmp_path): profile = load_bundled_hand_profile("l6_right_8") # Legacy v2 sessions retain their all-at-start verification waypoint API. - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2)) + profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, + directional_command_joints=frozenset())) pose = (230., 255., 255., 255., 255., 255.) waypoint = ReferenceWaypoint("show_tip", pose, {"front": (2,)}) profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,))) diff --git a/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py b/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py new file mode 100644 index 0000000..469890e --- /dev/null +++ b/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py @@ -0,0 +1,143 @@ +"""Rejected visual fits remain rejected and preserve useful raw-sample evidence.""" + +from copy import deepcopy +from dataclasses import asdict +import json +import math + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting import observed_motion +from linkerhand_calibration.core.fitting.rotation_diagnostics import RotationHoldoutFailure +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session + + +def rotation_rows(*, slip_axis=None, slip_one=False): + mount = Rotation.from_euler("xyz", (.24, -.17, .31)) + rows = [] + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for index, value in enumerate(np.linspace(0, 1, 65)): + rotation = mount * Rotation.from_euler("z", .8 * value) + if cycle == 3 and slip_axis and (not slip_one or + (direction == "decreasing" and index == 12)): + rotation *= Rotation.from_euler(slip_axis, math.radians(5)) + rows.append({"sample_id": f"camera_a:{cycle}:{direction}:{index}", + "view": "camera_a", "feedback_rad": float(value), + "relative_quaternion_xyzw": rotation.as_quat().tolist(), + "cycle": cycle, "direction": direction}) + return rows, mount.as_quat() + + +def fit(rows, reference, **context): + return observed_motion.fit_native_observed_rotation(rows, input_domain="feedback_rad", + input_to_joint_direction=1, reference_xyzw=reference, **context) + + +def test_optional_diagnostic_context_preserves_existing_success_outputs(): + rows, reference = rotation_rows() + original = deepcopy(rows) + curve, errors = fit(rows, reference) + contextual_curve, contextual_errors = fit(rows, reference, joint="hinge", view="camera_a") + assert asdict(curve) == asdict(contextual_curve) + assert errors == contextual_errors + assert max(errors) < 1e-12 + assert curve.angle_rad == pytest.approx(np.linspace(0, .8, 65), abs=1e-12) + assert rows == original + + +@pytest.mark.parametrize("axis", ["x", "z"]) +def test_diagnostics_distinguish_off_axis_slip_from_angle_error_without_refitting(axis, monkeypatch): + rows, reference = rotation_rows(slip_axis=axis) + original = deepcopy(rows) + training_calls = [] + primitive = observed_motion.fit_rotation_curve + + def record_training(observations, **kwargs): + training_calls.append(tuple(observations)) + return primitive(observations, **kwargs) + + monkeypatch.setattr(observed_motion, "fit_rotation_curve", record_training) + with pytest.raises(RotationHoldoutFailure) as caught: + fit(rows, reference, joint="hinge", view="camera_a") + error = caught.value + diagnostic = error.diagnostics + assert str(error).startswith("independent_rotation_holdout_failed:joint=hinge:view=camera_a:") + assert "max_deg=5.000000" in str(error) + assert diagnostic.full_so3.count == 130 + assert diagnostic.full_so3.mae_deg == pytest.approx(5, abs=1e-10) + assert diagnostic.full_so3.p95_deg == pytest.approx(5, abs=1e-10) + assert diagnostic.full_so3.maximum_deg == pytest.approx(5, abs=1e-10) + assert diagnostic.full_so3.passed is False + if axis == "z": + assert diagnostic.axial.mae_deg == pytest.approx(5, abs=1e-10) + assert diagnostic.off_axis.maximum_deg < 1e-10 + else: + assert diagnostic.off_axis.mae_deg > 4.9 + assert diagnostic.axial.maximum_deg < .05 + assert len(training_calls) == 1 + assert {row.cycle for row in training_calls[0]} == {0, 1, 2} + assert rows == original + + +def test_one_bad_holdout_image_is_reported_and_not_discarded(): + rows, reference = rotation_rows(slip_axis="z", slip_one=True) + with pytest.raises(RotationHoldoutFailure) as caught: + fit(rows, reference) + diagnostic = caught.value.diagnostics + assert diagnostic.full_so3.mae_deg < 1 + assert diagnostic.full_so3.p95_deg < 2 + assert diagnostic.full_so3.maximum_deg == pytest.approx(5, abs=1e-10) + assert diagnostic.worst_sample.sample_id == "camera_a:3:decreasing:12" + assert diagnostic.worst_sample.cycle == 3 + assert diagnostic.worst_sample.direction == "decreasing" + assert diagnostic.worst_sample.input_value == pytest.approx(12 / 64) + assert diagnostic.worst_sample.axial_error_deg == pytest.approx(5, abs=1e-10) + + +def test_finalizer_persists_joint_and_camera_evidence_before_any_spatial_solve(tmp_path, monkeypatch): + from measured_capture_fixture import measured_capture + from linkerhand_calibration.core.fitting import session + + profile, source, records, _ = measured_capture(tmp_path, layout="o6_right_8") + failed_joint = "rh_pinky_dip" + for row in records: + if row.get("joint") == failed_joint and row.get("cycle") == 3: + slipped = Rotation.from_quat(row["relative_quaternion_xyzw"]) * Rotation.from_euler("x", .1) + row["relative_quaternion_xyzw"] = slipped.as_quat().tolist() + parent = Rotation.from_quat(row["parent_pose_common"]["quaternion_xyzw"]) + row["child_pose_common"]["quaternion_xyzw"] = (parent * slipped).as_quat().tolist() + + def forbidden_spatial_solve(*args, **kwargs): + pytest.fail("a failed visual holdout must stop before spatial fitting") + + monkeypatch.setattr(session, "solve_urdf_zero_offsets", forbidden_spatial_solve) + previous = tmp_path / "previous" + previous.mkdir() + pointer = tmp_path / profile.artifacts.publication_pointer + pointer.symlink_to(previous, target_is_directory=True) + directory = tmp_path / "failed_session" + with pytest.raises(RotationHoldoutFailure) as caught: + finalize_profile_session(profile=profile, session_dir=directory, + serial_number="SYNTHETIC_DIAGNOSTIC", source_urdf=source, + protected_inputs={}, records=records) + + report = json.loads((directory / "fit_diagnostics.json").read_text()) + assert report["passed"] is False + assert report["publication_allowed"] is False + assert report["stage"] == "relative_rotation_holdout" + assert report["reason"] == str(caught.value) + detail = report["rotation_holdout"] + assert detail["joint"] == failed_joint + assert detail["view"] == "side" + assert detail["training_cycles"] == [0, 1, 2] + assert detail["holdout_cycle"] == 3 + assert detail["full_so3"]["maximum_deg"] > 3 + assert detail["worst_sample"]["sample_id"] in { + row["sample_id"] for row in records if row.get("joint") == failed_joint + and row.get("cycle") == 3 and row.get("sample_phase", "sweep") == "sweep"} + assert pointer.resolve() == previous + assert not list(directory.glob("*.urdf")) + assert not (directory / "release_manifest.json").exists() diff --git a/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py b/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py new file mode 100644 index 0000000..475fbb5 --- /dev/null +++ b/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py @@ -0,0 +1,295 @@ +"""Frozen Tag branches guard capture, zero references and coordinator races.""" + +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import replace +from threading import Event +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.core.geometry.pnp import SquareTagPose +from linkerhand_calibration.runtime.inputs import DetectionInput +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh +from pose_branch_fixture import IdealPoseTracker, confirm_reference_rows +from runtime_host_fixture import coordinator_fixture, ready +from test_joint_zero_runtime import reference_rows +from test_observation_capture import make_capture, frame + + +def branch_event(stamp, *, view="top", role="thumb_yaw", tag=7, reason="pose_branch_conflict"): + return {"kind": "pnp_branch_event", "view": view, "image_stamp_ns": stamp, + "tag_role": role, "tag_id": tag, "reason": reason, + "pnp_observation_evidence": {"candidate_diagnostics": { + "observation_stamp_ns": stamp, "branch_revision": 0, + "branch_status": "conflict", "branch_reason": reason, + "branch_confirmation_count": 5, "branch_frozen": True}}} + + +def begin_yaw(host, clock): + ready(host, clock) + assert host.start().success + host.execution.session.phase = Phase.SWEEP + host._motion = MotionCommand("sweep", host.profile.command.baseline_values, 40., + task_key="thumb_yaw_top", command_index=1, cycle=0, direction="decreasing") + + +@pytest.mark.parametrize("kind,paused", [ + ("required", True), ("fixed", True), ("fixed_projection", True), ("unstarted", False), + ("pending", False), ("ambiguous", False), ("fixed_unverified", False), ("recovering", False), ("stale_metadata", False), +]) +def test_branch_events_pause_only_current_required_roles_after_version_checks(tmp_path, monkeypatch, kind, paused): + host, clock = coordinator_fixture(tmp_path) + try: + begin_yaw(host, clock) + row = branch_event(host.ports.clock_ns()) + if kind in {"fixed", "fixed_projection"}: + row = branch_event(host.ports.clock_ns(), view="side", role="side_base", tag=3) + if kind == "fixed_projection": + row["pnp_observation_evidence"]["candidate_diagnostics"].update( + pose_constraint="fixed_reference", fixed_reference_verified=False, + fixed_reference_reprojection_error_px=2.1) + elif kind == "unstarted": + row = branch_event(host.ports.clock_ns(), view="front", role="thumb_pitch", tag=1) + elif kind == "pending": + row = branch_event(host.ports.clock_ns(), reason="pose_branch_unconfirmed") + elif kind == "ambiguous": + row = branch_event(host.ports.clock_ns(), reason="pose_branch_ambiguous") + elif kind == "fixed_unverified": + row = branch_event(host.ports.clock_ns(), view="front", role="front_base", tag=0, + reason="fixed_reference_pose_unverified") + row["pnp_observation_evidence"]["candidate_diagnostics"].update( + branch_status="fixed_reference_unavailable", pose_constraint="fixed_reference", + fixed_reference_verified=False, fixed_reference_reprojection_error_px=1.6233) + elif kind == "recovering": + row = branch_event(host.ports.clock_ns(), reason="pose_branch_recovery_required") + elif kind == "stale_metadata": + row["pnp_observation_evidence"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1 + monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((deepcopy(row),), None)) + host.receive_detections(DetectionInput(row["view"], host.ports.clock_ns(), ())) + assert host.raw_records[0]["kind"] == "session_start" + assert host.raw_records[1:] == [row] + assert host._unit_rows == host._steady_rows == [] + assert (host.execution.session.phase == Phase.PAUSED) is paused + if kind == "fixed_unverified": + assert ("front", "front_base") in host._branch_observation_reasons + yaw = next(t for t in host.profile.motion.tasks if t.key == "thumb_yaw_top") + front = next(t for t in host.profile.motion.tasks if t.view == "front") + assert host._task_branch_details(yaw) == "" + assert "fixed_reference_pose_unverified" in host._task_branch_details(front) + if paused: + assert f"view={row['view']}:tag={row['tag_id']}" in host.reason + expected_code = "OBS-BASE-POSE-110" if kind == "fixed_projection" else "OBS-POSE-BRANCH-109" + assert reason_zh({"reason": host.reason}, model_name="O6")[0] == expected_code + count = len(clock.positions) + host.tick() + assert len(clock.positions) == count + assert not host.finalization.started + finally: + host.close() + + +def test_late_branch_conflict_cannot_pause_or_append_after_abort(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + entered, release = Event(), Event() + try: + begin_yaw(host, clock) + def consume(frame, *a, **kw): + entered.set() + assert release.wait(2) + return (branch_event(frame.stamp_ns),), None + monkeypatch.setattr(host.capture, "consume", consume) + with ThreadPoolExecutor(1) as pool: + future = pool.submit(host.receive_detections, DetectionInput("top", host.ports.clock_ns(), ())) + assert entered.wait(2) + assert host.abort().success + release.set() + future.result(timeout=2) + assert host.execution.session.phase == Phase.ABORTED + assert [row["kind"] for row in host.raw_records] == ["session_start"] + assert not host._branch_observation_reasons + finally: + release.set() + host.close() + + +@pytest.mark.parametrize("invalid", ["mixed_revision", "missing_evidence", "stale_stamp", "wrong_role", "tracker_pending"]) +def test_joint_zero_cannot_freeze_unconfirmed_or_mixed_branch_evidence(tmp_path, invalid): + host, _ = coordinator_fixture(tmp_path) + try: + name = host.profile.motion.tasks[0].joints[0] + rows = reference_rows(host.profile, name, stamp=100) + confirm_reference_rows(host, rows) + if invalid == "mixed_revision": + rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["branch_revision"] += 1 + elif invalid == "missing_evidence": + rows[-1].pop("pnp_observation_evidence") + elif invalid == "stale_stamp": + rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1 + elif invalid == "wrong_role": + rows[-1]["pnp_observation_evidence"]["child"]["tag_role"] = "top_base" + else: + host.trackers["top"]._branches.states["thumb_yaw"].status = "pending" + host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in rows] + host._observation_epoch = host._segment_number = 1 + assert not host._lock_joint_zeros(SimpleNamespace(task_key=host.profile.motion.joint_zero_references[name].task_key, + zero_joints=(name,))) + expected = {"mixed_revision": "joint_zero_motion_evidence_changed", + "missing_evidence": "joint_zero_motion_evidence_incomplete", + "stale_stamp": "joint_zero_motion_evidence_unconfirmed", + "wrong_role": "joint_zero_motion_evidence_unconfirmed", + "tracker_pending": "joint_zero_motion_branch_not_confirmed"}[invalid] + assert expected in host._zero_reference_error + assert not host.execution.zero_references + assert all(not state.frozen for state in host.trackers["top"]._branches.states.values()) + finally: + host.close() + + +def test_joint_zero_freezes_both_physical_roles_with_one_revision(tmp_path): + host, _ = coordinator_fixture(tmp_path) + try: + name = host.profile.motion.tasks[0].joints[0] + rows = reference_rows(host.profile, name, stamp=100) + confirm_reference_rows(host, rows) + host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in rows] + host._observation_epoch = host._segment_number = 1 + assert host._lock_joint_zeros(SimpleNamespace(task_key=host.profile.motion.joint_zero_references[name].task_key, + zero_joints=(name,))) + assert set(host.trackers["top"]._branches.states) == {"top_base", "thumb_yaw"} + assert all(state.frozen for state in host.trackers["top"]._branches.states.values()) + assert len(host.execution.zero_references[name].samples) == 10 + finally: + host.close() + + +def test_fixed_pose_buffer_discards_old_revision_before_atomic_freeze(): + profile, lock, capture = make_capture() + class Sensor(IdealPoseTracker): + revision = 0 + def estimate(self, role, corners, *, stamp_ns, **kwargs): + pose = SquareTagPose((0., 0., 0., 1.), (0., 0., .45 + self.revision*.01), 0.) + self.accept_ideal_pose(role, pose, stamp_ns) + self._branches.states[role].revision = self.revision + self.last_candidate_diagnostics_by_role[role]["branch_revision"] = self.revision + return pose, "" + sensor = capture.trackers["front"] = Sensor() + lock.begin_session() + lock.start_locking() + for stamp in range(1, 10): + capture.consume(frame(profile, stamp, omit=("thumb_pitch", "thumb_dip"))) + sensor.revision = 1 + capture.consume(frame(profile, 10, omit=("thumb_pitch", "thumb_dip"))) + assert "front" not in capture.locked_poses + for stamp in range(11, 20): + capture.consume(frame(profile, stamp, omit=("thumb_pitch", "thumb_dip"))) + assert capture.locked_revisions == {"front": 1} + assert capture.locked_poses["front"].translation_xyz_m[2] == pytest.approx(.46) + assert sensor._branches.states["front_base"].frozen + + +def test_visible_but_quality_rejected_base_cannot_be_replaced_by_cache(): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(1, 15): + capture.consume(frame(profile, stamp)) + observation = replace(frame(profile, 20, omit=("front_base",)), observed_roles=frozenset({"front_base"})) + motion = MotionCommand("sweep", profile.command.baseline_values, 40., + "thumb_pitch_dip_front", 0, 0, "decreasing") + rows, _ = capture.consume(observation, motion) + assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) + + +@pytest.mark.parametrize("recovery_evidence", ["event", "sample"]) +def test_recovered_observation_clears_pending_error_and_final_quality_details(tmp_path, monkeypatch, recovery_evidence): + host, clock = coordinator_fixture(tmp_path) + try: + begin_yaw(host, clock) + row = branch_event(host.ports.clock_ns(), reason="pose_branch_recovery_required") + monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((deepcopy(row),), None)) + host.receive_detections(DetectionInput("top", host.ports.clock_ns(), ())) + task = host.profile.motion.tasks[0] + assert "top:tag=7:pose_branch_recovery_required" in host._task_branch_details(task) + assert host.execution.session.phase == Phase.SWEEP + clock.now += .04 + stamp = host.ports.clock_ns() + diagnostics = {"observation_stamp_ns": stamp, "branch_revision": 0, "branch_status": "tracking"} + if recovery_evidence == "event": + row = branch_event(stamp, reason="pose_branch_recovered") + row["pnp_observation_evidence"]["candidate_diagnostics"] = diagnostics + else: + row = {"kind": "joint_sample", "view": "top", "image_stamp_ns": stamp, + "pnp_observation_evidence": {"child": {"tag_role": "thumb_yaw", "candidate_diagnostics": diagnostics}}} + host.receive_detections(DetectionInput("top", stamp, ())) + assert not host._branch_observation_reasons + assert host._task_branch_details(task) == "" + assert host.execution.session.phase == Phase.SWEEP + finally: + host.close() + + +def test_pending_fixed_branch_cannot_use_cache_when_next_image_is_missing(): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for stamp in range(1, 15): + capture.consume(frame(profile, stamp)) + tracker = capture.trackers["front"] + tracker._branches.states["front_base"].status = "pending" + motion = MotionCommand("sweep", profile.command.baseline_values, 40., + "thumb_pitch_dip_front", 0, 0, "decreasing") + rows, _ = capture.consume(frame(profile, 20, omit=("front_base",)), motion) + assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) + + +def test_occluded_fixed_corners_discard_dependent_samples_until_stable_recovery(): + profile, lock, capture = make_capture() + capture.profile = profile = replace(profile, measurement=replace(profile.measurement, + candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"}))) + lock.begin_session() + lock.start_locking() + for stamp in range(1, 15): + capture.consume(frame(profile, stamp)) + original = capture.locked_reference("front") + motion = MotionCommand("sweep", profile.command.baseline_values, 40., + "thumb_pitch_dip_front", 0, 0, "decreasing") + for stamp in range(20, 30): + observation = frame(profile, stamp) + observation.corners["front_base"][0] += [0., 4.] + rows, movement = capture.consume(observation, motion) + assert movement is None # Local corner damage, below the drift gate. + assert len(rows) == 1 and rows[0]["kind"] == "pnp_branch_event" + assert rows[0]["reason"] == "fixed_reference_pose_unverified" + assert rows[0]["motion_phase"] == "sweep" + assert rows[0]["command_vector"] == list(observation.command) + assert rows[0]["feedback_vector"] == list(observation.feedback) + assert capture.locked_reference("front") == original + # Missing detection must not masquerade as a recovered visible reference. + rows, _ = capture.consume(frame(profile, 30, omit=("front_base",)), motion) + assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) + for stamp in range(31, 36): + rows, movement = capture.consume(frame(profile, stamp), motion) + samples = [r for r in rows if r.get("joint") == "rh_thumb_cmc_pitch"] + assert movement is None + assert bool(samples) is (stamp == 35) + if samples: + assert samples[0]["pnp_observation_evidence"]["parent"]["pose_source"] == "verified_fixed_reference" + assert capture.locked_reference("front") == original + + +def test_true_fixed_drift_in_unused_view_still_pauses_current_task(tmp_path, monkeypatch): + from linkerhand_calibration.runtime.reference_lock import ReferenceMovement + host, clock = coordinator_fixture(tmp_path) + try: + begin_yaw(host, clock) + movement = ReferenceMovement("front", 0, 6.0, 10) + monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((), movement)) + host.receive_detections(DetectionInput("front", host.ports.clock_ns(), ())) + assert host.execution.session.phase == Phase.PAUSED + assert host.reason == "fixed_reference_moved:front:6.0" + assert not host.finalization.started + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_runtime_readiness.py b/src/linkerhand_calibration/test/test_runtime_readiness.py index 6aa334d..b6b5efb 100644 --- a/src/linkerhand_calibration/test/test_runtime_readiness.py +++ b/src/linkerhand_calibration/test/test_runtime_readiness.py @@ -14,6 +14,7 @@ from linkerhand_calibration.runtime.status import normalize_status, render_statu def test_missing_stale_and_recovered_camera_pipeline_are_named_without_tag_gate(): profile = load_bundled_hand_profile("o6_right_8") host = SimpleNamespace(profile=profile, command_count=6, latest_feedback=(0,)*6, + required_observation_views=profile.vision.view_names, ports=SimpleNamespace(command_publisher_count=lambda: 1), sdk_adapter=SimpleNamespace(health=lambda: HardwareHealth(True, True)), cameras=SimpleNamespace(matrices={"front": True, "side": True, "top": True}, @@ -37,7 +38,10 @@ def test_missing_stale_and_recovered_camera_pipeline_are_named_without_tag_gate( def test_steady_checkpoint_does_not_erase_direction_statistics(): - host = SimpleNamespace(_steady_rows=[], _unit_rows=[], _capture_unit_key=None) + from linkerhand_calibration.runtime.branch_initialization import BranchInitialization + host = SimpleNamespace(_steady_rows=[], _unit_rows=[], _capture_unit_key=None, + branch_initialization=BranchInitialization(load_bundled_hand_profile("o6_right_8")), + _observation_epoch=1, _segment_number=1) motion = MotionCommand("sweep", (0.,), 1., task_key="test", command_index=0, cycle=0, direction="decreasing") begin = MethodType(CalibrationCoordinator._begin_motion_capture, host) diff --git a/src/linkerhand_calibration/test/test_separate_mapping_capture.py b/src/linkerhand_calibration/test/test_separate_mapping_capture.py new file mode 100644 index 0000000..d1644e0 --- /dev/null +++ b/src/linkerhand_calibration/test/test_separate_mapping_capture.py @@ -0,0 +1,139 @@ +"""Continuous four-round motion and independent move-and-hold mapping.""" + +from collections import defaultdict +from dataclasses import replace + +import numpy as np +import pytest + +from linkerhand_calibration.core.domain.sampling import command_nodes +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit +from runtime_host_fixture import coordinator_fixture, ready +from test_diagnostic_coordinator import advance, TERMINAL_PHASES +from test_single_pass_capture import CapturedFinalization + + +def separate(profile): + return replace(profile, acquisition=replace(profile.acquisition, + command_capture_mode="separate", steady_training_nodes=9, steady_extra_training_nodes={})) + + +def test_four_continuous_rounds_and_separate_mapping_reach_finalization(tmp_path, monkeypatch): + original = CalibrationEngine.scan_units + monkeypatch.setattr(CalibrationEngine, "scan_units", lambda engine: + tuple(unit for unit in original(engine) if unit.task_key == "thumb_yaw_top")) + finalizer = CapturedFinalization() + host, clock = coordinator_fixture(tmp_path, finalization=finalizer, profile_transform=separate) + ready(host, clock) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + assert host.start().success + entries, seen = defaultdict(list), set() + try: + for _ in range(9000): + if finalizer.started: + break + assert host.execution.session.phase not in TERMINAL_PHASES, host.reason + advance(host, clock) + motion = host._motion + if motion is not None and motion.phase in {"sweep", "steady"} and host._segment_number not in seen: + seen.add(host._segment_number) + entries[(motion.cycle, motion.direction)].append(motion) + assert finalizer.started and host.execution.session.phase == Phase.FIT, host.reason + assert len(host.execution.completed_units) == 12 + units = host.execution.session.engine.scan_units() + records = finalizer.inputs[0].records + spans = {} + for unit in units: + motions = entries[(unit.cycle, unit.direction)] + if unit.sample_phase == "sweep": + assert len(motions) == 1 and motions[0].phase == "sweep" + assert motions[0].target[motions[0].command_index] == unit.end + else: + assert all(m.phase == "steady" for m in motions) + plan = host.execution.sampling_plans[unit.task_key] + assert len(motions) == (len(plan["training_nodes"]) if unit.cycle == 4 else 10) + quality = evaluate_capture_unit(host.profile, unit, 1, records, first_cycle_spans=spans) + assert quality.passed, quality.failures + samples = [r for r in records if r.get("kind") == "joint_sample"] + moving = [r for r in samples if r["sample_phase"] == "sweep"] + steady = [r for r in samples if r["sample_phase"] == "steady"] + assert {r["cycle"] for r in moving} == {0, 1, 2, 3} + assert {r["cycle"] for r in steady} == {4, 5} + assert {r["image_stamp_ns"] for r in moving}.isdisjoint(r["image_stamp_ns"] for r in steady) + assert all(r["command_u8"] == r["steady_target"] for r in steady) + assert len(host.execution.sampling_plans["thumb_yaw_top"]["training_nodes"]) < 9 + from linkerhand_calibration.core.fitting.command_sampling import validate_sampling_plans + validate_sampling_plans(host.profile, records) + assert any(r.get("kind") == "preparation_reused_settled_pose" for r in records) + incomplete = [r for r in records if not (r.get("sample_phase") == "steady" + and r.get("cycle") == 5 and r.get("steady_index") == 1)] + assert not evaluate_capture_unit(host.profile, units[-1], 1, incomplete, first_cycle_spans=spans).passed + assert not list(tmp_path.rglob("release_manifest.json")) + monkeypatch.setattr(host.capture, "_motion_model_schedule", + lambda *_: pytest.fail("finalization must not continue solving moving joints")) + advance(host, clock) + assert host.execution.session.phase == Phase.FIT + from test_diagnostic_coordinator import projected_frame + for _ in range(host.profile.acquisition.fixed_reference_confirmation_frames): + clock.now += .04 + frame = projected_frame(host, "top", host.last_command) + moved = tuple(replace(tag, corners=tuple((x + 10, y) for x, y in tag.corners)) + if tag.tag_id == 6 else tag for tag in frame.detections) + host.receive_detections(replace(frame, detections=moved)) + # Acquisition has closed. These later images are outside the frozen + # dataset and must not interrupt its numerical/file validation. + assert host.execution.session.phase == Phase.FIT + assert not any(row.get("kind") == "paused" for row in host.raw_records) + finally: + host.close() + + +def test_extra_endpoint_node_keeps_validation_independent(): + from linkerhand_calibration.profiles import load_bundled_hand_profile + profile = separate(load_bundled_hand_profile("o6_right_8")) + profile = replace(profile, acquisition=replace(profile.acquisition, + steady_extra_training_nodes={"pinky_pitch_dip_side": (239.,)})) + task = next(t for t in profile.motion.tasks if t.key == "pinky_pitch_dip_side") + training = set(command_nodes(profile, task)) + validation = set(command_nodes(profile, task, holdout=True)) + assert 239 in training and 247 in validation + assert training & validation == {0., 255.} + assert len(training) == 10 and len(validation) == 11 + + +@pytest.mark.parametrize("model", ["o6", "g20"]) +@pytest.mark.parametrize("changed", ["feedback", "age", "target", "command", "phase", "missing"]) +def test_preparation_cannot_reuse_changed_or_expired_settling(tmp_path, monkeypatch, model, changed): + from linkerhand_calibration.runtime.motion_execution import MotionCommand + host, clock = coordinator_fixture(tmp_path, model=model) + ready(host, clock) + target = tuple(host.profile.command.baseline_values) + feedback = tuple(host.latest_feedback) + host.last_command = target + host._settled_pose = (target, feedback, clock.now) + motion = MotionCommand("prepare", target, 1.) + delta = .002 if host.command_unit == "rad" else 2. + altered = (target[0] + delta, *target[1:]) + if changed == "feedback": + clock.now += .01 + host.receive_feedback(host.profile.command.names, + (feedback[0] + delta, *feedback[1:]), host.ports.clock_ns()) + elif changed == "age": + clock.now += 1.01 + elif changed == "target": + motion = replace(motion, target=altered) + elif changed == "command": + host.last_command = altered + elif changed == "phase": + motion = replace(motion, phase="zero_approach") + else: + host._settled_pose = None + monkeypatch.setattr(host.execution, "motion_complete", + lambda: pytest.fail("this preparation must run its normal motion and settling checks")) + try: + assert not host._reuse_settled_preparation(motion, clock.now) + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_session_execution.py b/src/linkerhand_calibration/test/test_session_execution.py index 945c6d5..9dd7a19 100644 --- a/src/linkerhand_calibration/test/test_session_execution.py +++ b/src/linkerhand_calibration/test/test_session_execution.py @@ -1,5 +1,6 @@ """Virtual-clock tests use the same effect driver intended for the ROS host.""" +import math import pytest @@ -8,18 +9,22 @@ from linkerhand_calibration.runtime.execution import SessionExecution from linkerhand_calibration.runtime.session import CalibrationPhase as Phase from linkerhand_calibration.runtime.motion_execution import MotionExecution from linkerhand_calibration.runtime.scan_quality import observation_streams +from linkerhand_calibration.runtime.steady import steady_targets @pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"]) -def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout): +@pytest.mark.parametrize("capture_mode", ["interleaved", "separate"]) +def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout, capture_mode): profile = load_bundled_hand_profile(layout) from dataclasses import replace - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2)) + profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode=capture_mode)) + profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, + directional_command_joints=frozenset())) driver = SessionExecution(profile) driver.session.device_ready() driver.session.start() command = profile.command.baseline_values - rows, stamp, failed_once, swept = [], 0, False, [] + rows, stamp, failed_once, directions = [], 0, False, {} for _ in range(4000): phase = driver.session.phase if phase == Phase.FIT: @@ -32,31 +37,85 @@ def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout): continue motion = driver.motion(command) assert motion is not None, phase + if phase in {Phase.SWEEP, Phase.RESCAN}: + assert motion.phase != "steady_prepare" + initial = command command = motion.target if motion.recording: - if motion.phase == "sweep": - swept.append(motion) + unit = driver.action.scan_unit + task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + key = (unit.task_key, unit.cycle, unit.direction, motion.attempt) + directions.setdefault(key, []).append((unit, initial, motion)) # The first direction fails once; no speed reduction or extra retry. + # Omit its first steady point, keeping all moving observations. if not failed_once: failed_once = True else: - unit = driver.action.scan_unit - task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + start = initial[task.command_index] + end = command[task.command_index] + count = (max(3, math.ceil(128*abs(end-start)/abs(unit.end-unit.start))+1) + if motion.phase == "sweep" else profile.acquisition.steady_minimum_samples) for field, joint, spec in observation_streams(profile, task): - for index in range(129 if motion.phase == "sweep" else 3): + for index in range(count): stamp += 1 + feedback = start+(end-start)*index/(count-1) if motion.phase == "sweep" else end rows.append({field: joint, "view": spec.view, "task_name": task.key, "cycle": unit.cycle, "direction": unit.direction, "attempt": motion.attempt, "image_stamp_ns": stamp, "sample_phase": motion.phase, "steady_index": motion.steady_index, "steady_target": motion.target[task.command_index], - f"feedback_{profile.command.unit}": unit.start+(unit.end-unit.start)*index/128}) + f"feedback_{profile.command.unit}": feedback}) driver.motion_complete() assert driver.session.phase == Phase.FIT - assert len(driver.completed_units) == len(profile.motion.tasks)*8 - assert len(swept) == len(profile.motion.tasks)*8+1 - assert swept[0].speed == swept[1].speed - assert swept[1].attempt == 2 + unit_count = 12 if capture_mode == "separate" else 8 + assert len(driver.completed_units) == len(profile.motion.tasks)*unit_count + assert len(directions) == len(profile.motion.tasks)*unit_count+1 + for key, trace in directions.items(): + unit = trace[0][0] + task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + points = steady_targets(profile, unit) + if capture_mode == "interleaved": + assert [motion.phase for _, _, motion in trace] == ["steady"] + ["sweep", "steady"]*(len(points)-1) + elif unit.sample_phase == "sweep": + assert len(trace) == 1 and trace[0][2].phase == "sweep" + points = () + else: + assert all(motion.phase == "steady" for _, _, motion in trace) + held = [motion for _, _, motion in trace if motion.phase == "steady"] + assert tuple(motion.target[task.command_index] for motion in held) == points + assert [motion.steady_index for motion in held] == list(range(len(points))) + assert trace[0][1][task.command_index] == pytest.approx(unit.start) + assert trace[-1][2].target[task.command_index] == pytest.approx(unit.end) + travelled = 0.0 + sign = 1.0 if unit.direction == "increasing" else -1.0 + for _, initial, motion in trace: + delta = motion.target[task.command_index]-initial[task.command_index] + if motion.phase == "sweep": + assert sign*delta > 0 + elif capture_mode == "interleaved": + assert delta == pytest.approx(0.) + else: + assert sign*delta >= 0 + assert all(motion.target[i] == initial[i] for i in range(profile.command.command_count) + if i != task.command_index) + assert motion.speed == unit.speed + travelled += abs(delta) + assert travelled == pytest.approx(abs(unit.end-unit.start)) + for task in profile.motion.tasks: + units = [trace[0][0] for key, trace in directions.items() if key[0] == task.key and key[3] == 1] + assert len(units) == unit_count + assert len([unit for unit in units if unit.cycle < 4]) == 8 + assert all({unit.direction for unit in units if unit.cycle == cycle} + == {"increasing", "decreasing"} for cycle in (0, 1, 2, 3)) + distance = sum(abs(motion.target[task.command_index]-initial[task.command_index]) + for key, trace in directions.items() if key[0] == task.key and key[3] == 1 + for _, initial, motion in trace) + assert distance == pytest.approx(unit_count*abs(task.end_value-task.start_value)) + retried = [key for key in directions if key[3] > 1] + assert len(retried) == 1 and retried[0][3] == 2 + first_key = (*retried[0][:3], 1) + assert [motion.speed for _, _, motion in directions[first_key]] == [ + motion.speed for _, _, motion in directions[retried[0]]] def test_feedback_bias_does_not_turn_completed_travel_into_a_stall(): diff --git a/src/linkerhand_calibration/test/test_single_pass_capture.py b/src/linkerhand_calibration/test/test_single_pass_capture.py new file mode 100644 index 0000000..3e8cfeb --- /dev/null +++ b/src/linkerhand_calibration/test/test_single_pass_capture.py @@ -0,0 +1,151 @@ +"""A formal task completes four round trips through real image capture. + +Only the task scope and finalization output are replaced. The production +coordinator, frozen image geometry, SDK adapter and sample gates run unchanged. +Synthetic observations verify the flow; they are not a hand accuracy result. +""" + +from collections import defaultdict + +import numpy as np +import pytest + +from linkerhand_calibration.core.domain.sampling import command_nodes +from linkerhand_calibration.core.fitting.observed_motion import fit_native_observed_rotation +from linkerhand_calibration.runtime.acquisition import accepted_joint_records +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready +from test_diagnostic_coordinator import advance, TERMINAL_PHASES + + +class CapturedFinalization: + """Keep the actual finalizer input without building or publishing files.""" + + def __init__(self): + self.started = False + self.inputs = [] + + def start(self, inputs): + assert not self.started + self.inputs.append(inputs) + self.started = True + + def poll(self, *_args): + return None + + def cancel(self): + pass + + def close(self): + pass + + +def test_formal_yaw_four_round_trips_use_one_pass_and_disjoint_sample_phases(tmp_path, monkeypatch): + original_units = CalibrationEngine.scan_units + monkeypatch.setattr(CalibrationEngine, "scan_units", lambda engine: + tuple(unit for unit in original_units(engine) if unit.task_key == "thumb_yaw_top")) + finalizer = CapturedFinalization() + from dataclasses import replace + host, clock = coordinator_fixture(tmp_path, finalization=finalizer, + profile_transform=lambda p: replace(p, acquisition=replace(p.acquisition, + command_capture_mode="interleaved", steady_extra_training_nodes={}))) + ready(host, clock) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + assert host.diagnostic_capture is None + assert host.branch_initialization.image_geometry is True + assert host.start().success + task = next(task for task in host.profile.motion.tasks if task.key == "thumb_yaw_top") + units = host.execution.session.engine.scan_units() + expected_units = [(cycle, direction) for cycle in range(4) + for direction in ("decreasing", "increasing")] + assert [(unit.cycle, unit.direction) for unit in units] == expected_units + entries, commands = defaultdict(list), defaultdict(list) + seen_segments, order, previous_key = set(), [], None + try: + for _ in range(9000): + if finalizer.started: + break + if host.execution.session.phase in TERMINAL_PHASES: + pytest.fail(f"premature capture exit: {host.state}: {host.reason}: {host.execution.last_quality}") + before, first_command = host._motion, len(clock.positions) + advance(host, clock) + motion = host._motion or before + if motion is None or motion.phase not in {"sweep", "steady"}: + continue + key = (motion.cycle, motion.direction) + if key != previous_key: + order.append(key) + previous_key = key + commands[key].extend(value[task.command_index] for value in clock.positions[first_command:]) + if host._motion is not None and host._segment_number not in seen_segments: + seen_segments.add(host._segment_number) + entries[key].append((motion.phase, motion.target[task.command_index], motion.steady_index)) + else: + pytest.fail(f"virtual formal capture timed out: {host.state}: {host.reason}") + + assert host.execution.session.phase == Phase.FIT and finalizer.started + assert order == expected_units + assert len(host.execution.completed_units) == 8 + assert len(finalizer.inputs) == 1 + records = finalizer.inputs[0].records + samples = [row for row in records if row.get("kind") == "joint_sample"] + assert {row["joint"] for row in samples} == {"rh_thumb_cmc_yaw"} + assert {row["attempt"] for row in samples} == {1} + assert {row["sample_phase"] for row in samples} == {"sweep", "steady"} + assert {row["pnp_observation_evidence"]["child"]["pose_source"] for row in samples} == { + "image_constrained_current_image"} + sweep_ids = {row["image_stamp_ns"] for row in samples if row["sample_phase"] == "sweep"} + steady_ids = {row["image_stamp_ns"] for row in samples if row["sample_phase"] == "steady"} + assert sweep_ids and steady_ids and sweep_ids.isdisjoint(steady_ids) + assert len(sweep_ids) + len(steady_ids) == len(samples) + zero_rows = [row for row in records if row.get("kind") == "joint_zero_reference"] + assert [row["joint"] for row in zero_rows] == ["rh_thumb_cmc_yaw"] + assert {row["cycle"] for row in records if row.get("kind") == "joint_zero_sample"} == {0} + + policy = host.profile.acquisition + assert policy.minimum_valid_samples == 40 and policy.minimum_bins == 32 + assert policy.maximum_unobserved_fraction == .0625 + first_spans = {} + for unit in units: + key = (unit.cycle, unit.direction) + targets = command_nodes(host.profile, task, holdout=unit.cycle == 3) + if unit.direction == "decreasing": + targets = tuple(reversed(targets)) + expected = [("steady", targets[0], 0)] + for index, target in enumerate(targets[1:], 1): + expected.extend((("sweep", target, None), ("steady", target, index))) + assert entries[key] == expected + values = np.asarray(commands[key]) + assert values[0] == unit.start and values[-1] == unit.end + sign = 1 if unit.direction == "increasing" else -1 + assert np.all(sign * np.diff(values) >= 0), key + quality = evaluate_capture_unit(host.profile, unit, 1, records, first_cycle_spans=first_spans) + assert quality.passed, quality.failures + for metrics in quality.metrics.values(): + assert metrics["valid_frames"] >= 40 + assert metrics["feedback_bins"] >= 32 + assert metrics["maximum_bin_gap"] <= metrics["allowed_maximum_bin_gap"] == 16 + for row in samples: + if (row["cycle"], row["direction"]) == key and row["sample_phase"] == "steady": + assert row["command_u8"] == row["steady_target"] == targets[row["steady_index"]] + + train_nodes = set(command_nodes(host.profile, task)) + validation_nodes = set(command_nodes(host.profile, task, holdout=True)) + assert train_nodes & validation_nodes == {0., 255.} + rotation_rows = accepted_joint_records(host.profile, records)["rh_thumb_cmc_yaw"] + reference = host.execution.zero_references["rh_thumb_cmc_yaw"] + curve, errors = fit_native_observed_rotation(rotation_rows, + input_domain=host.profile.curve_input_domain, input_to_joint_direction=-1, + reference_xyzw=reference.quaternion("top"), joint=reference.joint, view="top") + training = {row["sample_id"] for row in rotation_rows if row["cycle"] in {0, 1, 2}} + holdout = {row["sample_id"] for row in rotation_rows if row["cycle"] == 3} + assert set(curve.circle["training_sample_ids"]) == training + assert training.isdisjoint(holdout) and len(errors) == len(holdout) + assert all(tuple(value[i] for i in (0, 2, 3, 4, 5)) == (255.,) * 5 for value in clock.positions) + assert not host.final_json and not host.final_urdf + assert not list(tmp_path.rglob("*.urdf")) and not list(tmp_path.rglob("release_manifest.json")) + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_source_mimic_export.py b/src/linkerhand_calibration/test/test_source_mimic_export.py new file mode 100644 index 0000000..bb35c9f --- /dev/null +++ b/src/linkerhand_calibration/test/test_source_mimic_export.py @@ -0,0 +1,200 @@ +"""Source coupling is immutable; JSON motion and preview bounds are separate.""" + +from copy import deepcopy +from dataclasses import replace +import hashlib +import json +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +from test_independent_urdf_export import nonlinear_release, assert_robot_state_publisher_angles +from linkerhand_calibration.core.urdf.acceptance import validate_artifact_structure +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.limits import MotionRangeEvidence +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.core.urdf.source_mimic import ( + compile_source_mimic_envelopes, validate_source_mimic_elements, +) +from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( + rebuild_urdf_from_json, serialize_urdf_correction, +) + + +def nominal_evidence(model, name, bounds, *, delta=0.0, safety=None): + joint = model.joints[name] + return MotionRangeEvidence(name, name, True, (joint.lower, joint.upper), bounds, bounds, bounds, + delta, 0., (0, 1, 2), ("view:100", "view:200"), ("view:10",), + "calibrated_motion_envelope", "synthetic measured support", safety, + "synthetic hard safety range" if safety else "") + + +@pytest.fixture +def source_mimic_release(nonlinear_release): + source, rights, old_plan, payload, path = nonlinear_release + model = UrdfKinematicModel(source) + bounds = (model.joints["ip"].lower, model.joints["ip"].upper) + evidence = {"ip": nominal_evidence(model, "ip", bounds)} + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=old_plan.source_sha256, zero_offsets_rad={}, authorized_fields=rights, + measured_ranges_output_rad={"ip": bounds}, range_evidence=evidence, preserve_source_mimic=True) + payload = deepcopy(payload) + payload["urdf_correction"] = serialize_urdf_correction(plan) + path.write_text(json.dumps(payload)) + return source, rights, plan, payload, path + + +def rebuild(release, output): + source, rights, _, payload, path = release + path.write_text(json.dumps(payload)) + return rebuild_urdf_from_json(path, source, output, authorized_fields=rights, + expected_profile_id=payload["profile_id"]) + + +def test_near_zero_extension_preserves_source_coupling_and_measured_support(source_mimic_release, tmp_path): + source, rights, plan, payload, _ = source_mimic_release + before = source.read_bytes() + output = rebuild(source_mimic_release, tmp_path/"retained.urdf") + validate_source_mimic_elements(source, output) + model = UrdfKinematicModel(output) + assert payload["urdf_correction"]["schema_version"] == 4 + assert model.joints["ip"].mimic_multiplier == 1.86 + assert model.joints["ip"].mimic_offset == 0 + assert model.joints["ip"].lower == pytest.approx(1.86*model.joints["pitch"].lower) + assert plan.range_evidence["ip"].output_limits_rad[0] == -.0020103987491093 + assert plan.mimic_envelopes["ip"].base_limits_rad == plan.range_evidence["ip"].output_limits_rad + assert output.read_bytes() == plan.write(source, tmp_path/"direct.urdf").read_bytes() + assert source.read_bytes() == before + validate_artifact_structure(source_urdf=source, corrected_urdf=output, source_sha256=plan.source_sha256, + authorized_fields=rights, payload=payload, zero_offsets_rad={}, range_evidence=plan.range_evidence) + for command in (0, 64, 128, 255): + angles = {name: row["angle_rad"][command] for name, row in payload["joints"].items()} + assert model.resolve_angles(angles, independent_mimic_angles=True) == angles + assert model.resolve_angles({"pitch": angles["pitch"]})["ip"] == pytest.approx(1.86*angles["pitch"]) + assert payload["joints"]["ip"]["angle_rad"][-1] != pytest.approx(1.86*.6) + + +@pytest.mark.parametrize("mutation", ["multiplier", "offset", "remove", "envelope", "padding", "missing", "policy"]) +def test_json_cannot_edit_source_mimic_or_forge_envelope(source_mimic_release, tmp_path, mutation): + _, _, _, payload, _ = source_mimic_release + correction = payload["urdf_correction"] + if mutation in {"multiplier", "offset", "remove"}: + field = {"multiplier": "mimic_multiplier", "offset": "mimic_offset", "remove": "remove_mimic"}[mutation] + correction["joint_patches"]["ip"][field] = True if mutation == "remove" else "0.1" + elif mutation == "envelope": + correction["mimic_envelopes"]["ip"]["mimic_reachable_limits_rad"] = [-.5, 1.2] + elif mutation == "padding": + correction["joint_patches"]["ip"]["limit_lower"] = "-0.5" + correction["mimic_envelopes"]["ip"]["output_limits_rad"] = [-.5, 1.2] + elif mutation == "missing": + del correction["mimic_envelopes"]["ip"] + else: + correction["mimic_policy"] = "unknown" + output = tmp_path/"rejected.urdf" + with pytest.raises(ValueError): + rebuild(source_mimic_release, output) + assert not output.exists() + + +def test_union_cannot_be_presented_as_measured_support(source_mimic_release, tmp_path): + source, rights, plan, payload, _ = source_mimic_release + # Coherently changing both fields still needs trusted fit evidence at final acceptance. + correction = payload["urdf_correction"] + correction["mimic_envelopes"]["ip"]["base_limits_rad"] = [-.5, 1.2] + correction["mimic_envelopes"]["ip"]["output_limits_rad"] = [-.5, 1.2] + correction["joint_patches"]["ip"]["limit_lower"] = "-0.5" + output = rebuild(source_mimic_release, tmp_path/"candidate.urdf") + with pytest.raises(ValueError, match="differs_from_trusted_ranges"): + validate_artifact_structure(source_urdf=source, corrected_urdf=output, source_sha256=plan.source_sha256, + authorized_fields=rights, payload=payload, zero_offsets_rad={}, range_evidence=plan.range_evidence) + + +def test_hard_bounds_are_not_relaxed_to_retain_mimic(source_mimic_release): + source, rights, plan, _, _ = source_mimic_release + args = dict(source_urdf=source, source_sha256=plan.source_sha256, + zero_offsets_rad={}, authorized_fields=rights, preserve_source_mimic=True) + with pytest.raises(ValueError, match="source_mimic_exceeds_safety_range:ip"): + build_standard_correction_plan(**args) + measured = plan.range_evidence["ip"] + with pytest.raises(ValueError, match="source_mimic_exceeds_safety_range:ip"): + build_standard_correction_plan(**args, measured_ranges_output_rad={"ip": measured.output_limits_rad}, + range_evidence={"ip": replace(measured, independent_safety_limits_cad_rad=measured.output_limits_rad, + safety_evidence_source="actual hard bound")}) + + +def test_standard_ros_still_drives_original_passive_joint(source_mimic_release, tmp_path): + assert_robot_state_publisher_angles(source_mimic_release, tmp_path, use_mimic=True) + + +@pytest.mark.parametrize("multiplier,offset", [(-2., .1), (0., .2), (2., -.1)]) +def test_mimic_chains_use_reachable_parent_not_its_wider_json_envelope(tmp_path, multiplier, offset): + source = tmp_path/"chain.urdf" + source.write_text(f''' + + + + + + + + + + + +''') + model = UrdfKinematicModel(source) + ranges = {"active": (-.5, .5)} + offsets = {"active": .2, "passive": .3, "tip": -.4} + envelopes = compile_source_mimic_envelopes(model, ranges, offsets, {}) + expected = tuple(sorted((multiplier*-.5+offset, multiplier*.5+offset))) + assert envelopes["passive"].mimic_reachable_limits_rad == expected + assert envelopes["tip"].mimic_reachable_limits_rad == expected + assert envelopes["tip"].mimic_reachable_limits_rad != envelopes["passive"].output_limits_rad + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad=offsets, measured_ranges_output_rad=ranges, preserve_source_mimic=True, + authorized_fields={name: ("origin.rpy", "limit.lower", "limit.upper") for name in model.joints}) + output = plan.write(source, tmp_path/"chain-output.urdf") + validate_source_mimic_elements(source, output) + assert ET.parse(output).find(".//joint[@name='tip']/mimic").attrib == {"joint": "passive"} + corrected = UrdfKinematicModel(output) + for angle in np.linspace(-.5, .5, 20): + assert corrected.resolve_angles({"active": angle})["tip"] == pytest.approx(multiplier*angle+offset) + + +@pytest.mark.parametrize("relative", [ + "l6_left/linkerhand_l6v3.1_left.urdf", "l6_right/linkerhand_l6v3.1_right.urdf", + "o6_left/linkerhand_o6_left.urdf", "o6_right/linkerhand_o6_right.urdf", + "o12_left/linkerhand_o12_t3_left_urdf-0703.urdf", "o12_right/linkerhand_o12_t3_right-0703.urdf", + "g20_left/linkerhand_g20_left.urdf", "g20_right/linkerhand_g20_right.urdf", +]) +def test_each_vendor_source_preserves_its_own_coefficients_and_topology(relative, tmp_path): + source = Path(__file__).resolve().parents[1]/"urdf"/relative + model = UrdfKinematicModel(source) + before = source.read_bytes() + # Synthetic subranges avoid the known inconsistencies in full CAD ranges. + # This checks both sides' export contracts, not their hardware calibration. + ranges = {name: (j.lower*.25, j.upper*.25) for name, j in model.joints.items() + if j.kind != "fixed" and j.mimic_joint is None} + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(before).hexdigest(), zero_offsets_rad={}, + measured_ranges_output_rad=ranges, preserve_source_mimic=True, + authorized_fields={name: ("limit.lower", "limit.upper") for name in model.joints}) + output = plan.write(source, tmp_path/"output.urdf") + validate_source_mimic_elements(source, output) + assert source.read_bytes() == before + + +@pytest.mark.parametrize("relative,joint", [ + ("l6_right/linkerhand_l6v3.1_right.urdf", "rh_thumb_dip"), + ("o12_right/linkerhand_o12_t3_right-0703.urdf", "ring_pip"), + ("g20_right/linkerhand_g20_right.urdf", "index_dip"), +]) +def test_known_full_source_range_conflicts_are_explicit(relative, joint): + source = Path(__file__).resolve().parents[1]/"urdf"/relative + with pytest.raises(ValueError, match=f"source_mimic_exceeds_safety_range:{joint}:"): + build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad={}, authorized_fields={}, preserve_source_mimic=True) diff --git a/src/linkerhand_calibration/test/test_spatial_solve_order.py b/src/linkerhand_calibration/test/test_spatial_solve_order.py new file mode 100644 index 0000000..9e2146b --- /dev/null +++ b/src/linkerhand_calibration/test/test_spatial_solve_order.py @@ -0,0 +1,33 @@ +"""Recorded serial geometry must not depend on required-zero list order.""" + +from dataclasses import replace +from pathlib import Path +import gzip +import json +import math + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.fitting.session import compile_spatial_profile +from linkerhand_calibration.core.fitting.spatial import JointAxisMeasurement, solve_urdf_zero_offsets + + +def test_recorded_thumb_pitch_uses_the_fitted_upstream_yaw(): + package = Path(__file__).resolve().parents[1] + with gzip.open(package / "test/fixtures/o6_spatial_125641.json.gz", "rt") as stream: + values = json.load(stream) + values["measurements"] = [JointAxisMeasurement(**row) for row in values["measurements"]] + values["curves"] = {name: JointCurveFit(**row) for name, row in values["curves"].items()} + profile = load_bundled_hand_profile("o6_right_8") + spatial = compile_spatial_profile(profile) + values.update(source_urdf=package / "urdf/o6_right/linkerhand_o6_right.urdf") + wrong = solve_urdf_zero_offsets(**values, zero_profile=replace(spatial, + direct_zero_joints=("rh_pinky_mcp_pitch", "rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw"))) + assert not wrong.passed + assert max(wrong.validation_errors_rad) > math.radians(4) + corrected = solve_urdf_zero_offsets(**values, zero_profile=spatial) + assert corrected.passed, corrected.failure_reasons + assert max(corrected.validation_errors_rad) < math.radians(1) + reordered = replace(profile, zero=replace(profile.zero, + direct_zero_joints=tuple(reversed(profile.zero.direct_zero_joints)))) + assert compile_spatial_profile(reordered).direct_zero_joints == spatial.direct_zero_joints diff --git a/src/linkerhand_calibration/test/test_steady_feedback_window.py b/src/linkerhand_calibration/test/test_steady_feedback_window.py new file mode 100644 index 0000000..e52fc06 --- /dev/null +++ b/src/linkerhand_calibration/test/test_steady_feedback_window.py @@ -0,0 +1,143 @@ +"""Feedback stability must survive scheduler jitter without hiding motion.""" + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution + + +@pytest.fixture(params=["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) +def profile(request): + return load_bundled_hand_profile(request.param) + + +@pytest.fixture(params=["steady", "joint_zero"]) +def phase(request): + return request.param + + +def segment_at_baseline(profile, phase, *, target=None, started_at=0.): + task = profile.motion.tasks[0] + baseline = profile.command.baseline_values + speed = task.formal_speed_u8 if profile.command.unit == "u8" else task.formal_speed + return MotionExecution(profile, MotionCommand( + phase, baseline if target is None else target, speed, + task_key=task.key, command_index=task.command_index), + initial_command=baseline, initial_feedback=baseline, now=started_at, identity="test") + + +def observe(segment, now, *, stamp=None, feedback=None): + segment.sample(now) + segment.observe(segment.profile.command.baseline_values if feedback is None else feedback, + stamp=now if stamp is None else stamp, now=now) + return segment.steady_ready(now) + + +@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) +def test_constant_feedback_with_90ms_timer_becomes_and_remains_stable(profile, phase, started_at): + segment = segment_at_baseline(profile, phase, started_at=started_at) + for index in range(21): + now = started_at + index * .09 + stable = observe(segment, now) + if index < 3: + assert not stable + else: + assert stable, f"Constant feedback lost stability at {now:.2f} s" + + +@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) +def test_mixed_timer_intervals_do_not_revoke_constant_feedback_stability(profile, phase, started_at): + segment = segment_at_baseline(profile, phase, started_at=started_at) + times = [index * .02 for index in range(26)] + times += [.59, .68, .77, .79, .81, .83, .85, .87, .89, .91] + for elapsed in times: + now = started_at + elapsed + stable = observe(segment, now) + if elapsed >= .24: + assert stable, f"Scheduler jitter cleared stable feedback at {now:.2f} s" + + +@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) +def test_100hz_timer_preserves_stability_between_60hz_feedback_samples(profile, phase, started_at): + segment = segment_at_baseline(profile, phase, started_at=started_at) + stamps = set() + for tick in range(200): + # Feedback identities advance at 60 Hz. The 100 Hz timer often sees + # the same independent observation and must retain its stable window. + stamp = tick * 60 // 100 + stamps.add(stamp) + now = started_at + tick / 100 + stable = observe(segment, now, stamp=stamp) + if tick >= 24: + assert stable, f"A tick without new feedback revoked stability at {now:.2f} s" + assert len(segment.stamped_history) == len(stamps) < 200 + + +def test_actual_feedback_change_requires_a_new_stable_window(profile, phase): + segment = segment_at_baseline(profile, phase) + for now in (0., .09, .18, .27): + stable = observe(segment, now) + assert stable + + index = segment.command.command_index + changed = list(profile.command.baseline_values) + lower, upper = (profile.command.minimum_feedback_values[index], + profile.command.maximum_feedback_values[index]) + # A real encoder change well above the existing stability threshold. + delta = .05 * (upper - lower) + changed[index] += delta if changed[index] <= (lower + upper) / 2 else -delta + assert not observe(segment, .30, feedback=changed) + for now in (.39, .48, .57): + stable = observe(segment, now, feedback=changed) + assert stable + + +def test_repeated_feedback_stamp_cannot_supply_three_independent_samples(profile, phase): + segment = segment_at_baseline(profile, phase) + for now in (0., .10, .20): + assert not observe(segment, now, stamp=1) + for now in (.21, .24, .27): + assert not observe(segment, now, stamp=2) + assert len(segment.stamped_history) == 2 + + +def test_feedback_before_trajectory_end_cannot_establish_the_hold_window(profile, phase): + task = profile.motion.tasks[0] + index = task.command_index + target = list(profile.command.baseline_values) + target[index] = (profile.command.minimum_values[index] + profile.command.maximum_values[index]) / 2 + segment = segment_at_baseline(profile, phase, target=tuple(target)) + for now in (0., .01, .02): + assert not observe(segment, now) + assert segment.finished_at is None + arrival = segment.duration + .1 + for delay in (0., .02, .04): + assert not observe(segment, arrival + delay) + assert segment.finished_at == arrival + for delay in (.10, .20, .30): + stable = observe(segment, arrival + delay) + assert stable + + +def test_stale_feedback_history_cannot_remain_stable_indefinitely(profile, phase): + segment = segment_at_baseline(profile, phase) + for now in (0., .09, .18, .27): + stable = observe(segment, now) + assert stable + stale_time = .27 + profile.acquisition.steady_window_seconds + .01 + assert not segment.steady_ready(stale_time) + + +def test_joint_zero_checks_held_channels_in_addition_to_the_current_joint(profile): + segment = segment_at_baseline(profile, "joint_zero") + for now in (0., .09, .18, .27): + stable = observe(segment, now) + assert stable + index = next(index for index in range(profile.command.command_count) + if index != segment.command.command_index and index not in profile.command.disabled_indices) + changed = list(profile.command.baseline_values) + lower, upper = (profile.command.minimum_feedback_values[index], + profile.command.maximum_feedback_values[index]) + delta = .05 * (upper - lower) + changed[index] += delta if changed[index] <= (lower + upper) / 2 else -delta + assert not observe(segment, .30, feedback=changed) diff --git a/src/linkerhand_calibration/test/test_storage.py b/src/linkerhand_calibration/test/test_storage.py index c17c298..03419ad 100644 --- a/src/linkerhand_calibration/test/test_storage.py +++ b/src/linkerhand_calibration/test/test_storage.py @@ -1,9 +1,11 @@ from linkerhand_calibration.storage import ( append_jsonl, + append_jsonl_many, atomic_write_json, completed_scan_keys, load_json, load_jsonl, + sync_jsonl, ) @@ -33,3 +35,35 @@ def test_resume_ignores_only_a_truncated_final_jsonl_record(tmp_path) -> None: with raw_path.open("a", encoding="utf-8") as stream: stream.write('{"kind":"sample"\n\n') assert load_jsonl(raw_path) == [{"kind": "sample", "phase": "root"}] + + +def test_continuous_batches_flush_in_order_and_checkpoint_syncs_all_prior_rows(tmp_path, monkeypatch): + from linkerhand_calibration.core.artifacts import storage + path = tmp_path / "raw_samples.jsonl" + persisted = [] + monkeypatch.setattr(storage.os, "fsync", lambda _fd: persisted.append(load_jsonl(path))) + rows = [{"kind": "joint_sample", "image_stamp_ns": index} for index in range(3)] + append_jsonl_many(path, rows, durable=False) + assert load_jsonl(path) == rows + assert persisted == [] + checkpoint = {"kind": "scan_unit_complete", "passed": True} + append_jsonl(path, checkpoint) + assert persisted == [rows + [checkpoint]] + more = {"kind": "pnp_branch_event"} + append_jsonl_many(path, [more]) # Existing callers remain durable by default. + assert persisted[-1] == rows + [checkpoint, more] + sync_jsonl(path) + assert len(persisted) == 3 + assert persisted[-1] == load_jsonl(path) + + +def test_explicit_sync_failure_is_not_swallowed(tmp_path, monkeypatch): + import pytest + from linkerhand_calibration.core.artifacts import storage + path = tmp_path / "raw_samples.jsonl" + append_jsonl_many(path, [{"kind": "joint_sample"}], durable=False) + def failed(_fd): + raise OSError("simulated disk sync failure") + monkeypatch.setattr(storage.os, "fsync", failed) + with pytest.raises(OSError, match="disk sync failure"): + sync_jsonl(path) diff --git a/src/linkerhand_calibration/test/test_tag_border.py b/src/linkerhand_calibration/test/test_tag_border.py new file mode 100644 index 0000000..a0999b7 --- /dev/null +++ b/src/linkerhand_calibration/test/test_tag_border.py @@ -0,0 +1,110 @@ +"""Occluded decoded quads must not become fixed-reference movement evidence.""" + +from pathlib import Path + +import cv2 +import numpy as np + +from linkerhand_calibration.core.geometry.tag_border import inspect_tag_border +from linkerhand_calibration.runtime.image_pairing import ExactImagePairing +from linkerhand_calibration.runtime.reference_lock import ReferenceLock + + +def test_recorded_front_tag_border_distinguishes_clear_and_thumb_occlusion(): + fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") + for image, corners, expected in zip(fixture["images"], fixture["corners"], fixture["expected"]): + assert inspect_tag_border(image, corners)["passed"] == bool(expected) + + +def test_visible_translation_still_triggers_original_five_pixel_movement_guard(): + fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") + image, corners = fixture["images"][0], fixture["corners"][0] + shifted = cv2.warpAffine(image, np.float32([[1, 0, 12], [0, 1, 8]]), + (image.shape[1], image.shape[0])) + moved = corners + [12, 8] + assert inspect_tag_border(shifted, moved)["passed"] + lock = ReferenceLock({"front": 0}) + lock.begin_session(); lock.start_locking() + for _ in range(10): lock.observe("front", 0, corners) + for _ in range(9): assert lock.observe("front", 0, moved) is None + assert lock.observe("front", 0, moved).drift_px > 5 + + +def test_blank_and_outside_image_have_no_border_evidence(): + corners = np.float32([[30, 30], [90, 30], [90, 90], [30, 90]]) + assert not inspect_tag_border(np.full((120, 120), 100, np.uint8), corners)["passed"] + assert not inspect_tag_border(np.full((120, 120), 100, np.uint8), corners-40)["passed"] + + +def test_pairing_requires_same_stamp_in_either_order_and_never_reuses_old_images(): + pair = ExactImagePairing(capacity=2) + assert pair.add("detections", 10, "d10") is None + assert pair.add("image", 11, "i11") is None + assert pair.add("image", 10, "i10") == ("i10", "d10") + assert pair.add("detections", 11, "d11") == ("i11", "d11") + assert pair.add("image", 10, "old") is None + assert pair.add("detections", 11, "duplicate") is None + for stamp in (12, 13, 14): pair.add("image", stamp, f"i{stamp}") + assert len(pair.images) == 2 + assert pair.add("detections", 12, "expired") is None + assert pair.add("detections", 14, "d14") == ("i14", "d14") + + +def test_coordinator_discards_occluded_quad_but_pauses_on_visible_motion(tmp_path): + from runtime_host_fixture import coordinator_fixture, ready + from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection + from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + host, clock = coordinator_fixture(tmp_path) + try: + ready(host, clock) + assert host.start().success + corners = np.float32([[100, 160], [160, 160], [160, 100], [100, 100]]) + host.reference_lock.start_locking() + for view, tag_id in host.reference_lock.fixed_tag_by_view.items(): + for _ in range(10): host.reference_lock.observe(view, tag_id, corners) + for passed in (False, True): + for _ in range(10): + clock.now += .04 + detection = TagDetection(0, tuple(map(tuple, corners+[12, 8])), 0, 100., + {"passed": passed, "reason": "" if passed else "tag_border_incomplete"}) + host.receive_detections(DetectionInput("front", host.ports.clock_ns(), (detection,))) + if not passed: + assert host.execution.session.phase != Phase.PAUSED + assert host.reference_lock.latest_drift_px["front"] == 0 + assert sum(r["kind"] == "detection_quality_rejected" for r in host.raw_records) == 10 + assert not any(r["kind"] in {"joint_sample", "joint_zero_sample"} for r in host.raw_records) + assert host.execution.session.phase == Phase.PAUSED + assert host.reason.startswith("fixed_reference_moved:front:") + finally: + host.close() + + +def test_image_process_publishes_checked_pixels_with_exact_identity(monkeypatch, tmp_path): + import rclpy + from types import SimpleNamespace + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.ros.tag_border_filter import TagBorderFilter + from linkerhand_calibration.runtime.ros.io import checked_detection_input + monkeypatch.setenv("ROS_DOMAIN_ID", "221") + monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path/"logs")) + fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") + rclpy.init() + node = TagBorderFilter(load_bundled_hand_profile("o6_right_8"), ("front",)) + sent = [] + monkeypatch.setattr(node.publishers_by_view["front"], "publish", sent.append) + try: + for index in (0, 5): + image = node.bridge.cv2_to_imgmsg(fixture["images"][index], encoding="mono8") + image.header.stamp.sec = index+1 + detection = SimpleNamespace(header=image.header, detections=[SimpleNamespace( + id=0, hamming=0, decision_margin=100, + corners=[SimpleNamespace(x=float(x), y=float(y)) for x, y in fixture["corners"][index]])]) + node.receive("front", "detections", detection) + assert len(sent) == (0 if index == 0 else 1) + node.receive("front", "image", image) + observed = checked_detection_input("front", sent[-1], fixed_ids={0}) + assert observed.stamp_ns == (index+1)*1_000_000_000 + assert observed.detections[0].border_quality["passed"] == (index == 0) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/linkerhand_calibration/test/test_transfer_artifact_contract.py b/src/linkerhand_calibration/test/test_transfer_artifact_contract.py index bbc72b0..475f99d 100644 --- a/src/linkerhand_calibration/test/test_transfer_artifact_contract.py +++ b/src/linkerhand_calibration/test/test_transfer_artifact_contract.py @@ -48,6 +48,8 @@ def transfer_artifacts(tmp_path): command=SimpleNamespace(unit="u8", names=("source", "target"), baseline_values=(0, 0), feedback_by_index=True, feedback_name_aliases={}), measurement=SimpleNamespace(transferred_motion_sources=transfers), + artifacts=SimpleNamespace(directional_command_joints=frozenset()), + acquisition=SimpleNamespace(command_capture_mode="interleaved"), zero=SimpleNamespace(known_baseline_geometry={}, cad_zero_assumptions=assumptions, transferred_zero_sources={"target": "source"})) knots = tuple(float(i) for i in range(0, 256, 51)) diff --git a/src/linkerhand_calibration/test/test_unified_engine.py b/src/linkerhand_calibration/test/test_unified_engine.py index 58e22f6..bca8644 100644 --- a/src/linkerhand_calibration/test/test_unified_engine.py +++ b/src/linkerhand_calibration/test/test_unified_engine.py @@ -52,7 +52,10 @@ def test_all_product_profiles_use_unified_engine_and_sdk_adapter() -> None: adapter = registered.build_sdk_adapter() assert engine.checkpoint_token == ACQUISITION_POLICY_VERSION assert adapter.command_layout is registered.profile.command - assert len(engine.scan_units()) == len(registered.profile.motion.tasks) * 8 + units = engine.scan_units() + assert len([u for u in units if u.sample_phase == "sweep"]) == len(registered.profile.motion.tasks) * 8 + separate = registered.profile.acquisition.command_capture_mode == "separate" + assert len([u for u in units if u.sample_phase == "steady"]) == len(registered.profile.motion.tasks) * (4 if separate else 0) def test_only_o12_has_one_three_degree_mapping_probe() -> None: @@ -195,6 +198,7 @@ def test_o12_internal_blind_spot_larger_than_one_sixteenth_is_rejected() -> None def test_old_checkpoint_policy_is_intentionally_incompatible() -> None: + from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION profile = next( item.profile for item in get_default_registry() if item.profile.key.layout == "g20_right_19" @@ -204,6 +208,8 @@ def test_old_checkpoint_policy_is_intentionally_incompatible() -> None: assert engine.resume_compatible({ "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "capture_schedule_version": engine.capture_schedule_version, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, }) diff --git a/src/linkerhand_calibration/test/test_unified_launch.py b/src/linkerhand_calibration/test/test_unified_launch.py index 176040a..f5c9b52 100644 --- a/src/linkerhand_calibration/test/test_unified_launch.py +++ b/src/linkerhand_calibration/test/test_unified_launch.py @@ -7,8 +7,10 @@ from pathlib import Path import pytest import yaml from launch import LaunchContext -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.utilities import perform_substitutions +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.utilities import evaluate_parameters, to_parameters_list from ros2launch.api.api import parse_launch_arguments from linkerhand_calibration.product import load_product_config @@ -62,8 +64,8 @@ def test_actual_launch_builds_from_protected_profile(tmp_path, model, renamed_vi if isinstance(action, DeclareLaunchArgument): action.execute(context) actions = module._launch_stack(context) - # Cameras, component container, SDK, calibration owner; none is executed. - assert sum(isinstance(action, Node) for action in actions) == 6 + # Cameras, detector container, border filter, SDK and calibration owner. + assert sum(isinstance(action, Node) for action in actions) == 7 assert not (tmp_path / "session/raw_samples.jsonl").exists() @@ -76,3 +78,104 @@ def test_launch_rejects_missing_profile_before_creating_processes(): action.execute(context) with pytest.raises(RuntimeError, match="protected YAML Profile"): module._launch_stack(context) + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +def test_detector_receives_protected_model_settings(tmp_path, monkeypatch, model): + """Resolve the actual component parameters, including YAML/override order.""" + config = load_product_config(PACKAGE / f"config/{model}_right_product.yaml", + workspace=PACKAGE.parents[1], check_can=False) + context = LaunchContext() + context.launch_configurations.update(dict(parse_launch_arguments( + launch_command(config, tmp_path / "session", record_bag=False, + commands_enabled=False)[4:]))) + module = launch_module() + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + components = [] + + def capture_container(**kwargs): + components.extend(kwargs["composable_node_descriptions"]) + return ComposableNodeContainer(**kwargs) + + monkeypatch.setattr(module, "ComposableNodeContainer", capture_container) + module._launch_stack(context) + protected = yaml.safe_load(config.tag_config.read_text()) + checked = 0 + for component in components: + if perform_substitutions(context, component.node_plugin) != "AprilTagNode": + continue + name = perform_substitutions(context, component.node_name) + namespace = perform_substitutions(context, component.node_namespace) + effective = {parameter.name: parameter.value for parameter in to_parameters_list( + context, name, namespace, evaluate_parameters(context, component.parameters))} + expected = protected[f"{namespace}/{name}"]["ros__parameters"] + assert effective["detector.decimate"] == expected["detector"]["decimate"] + assert tuple(effective["tag.ids"]) == tuple(expected["tag"]["ids"]) + assert tuple(effective["tag.sizes"]) == tuple(expected["tag"]["sizes"]) + checked += 1 + assert checked == len(config.calibration_contract.typed_profile.vision.views) + + +@pytest.mark.parametrize("renamed_views", [False, True]) +def test_thumb_launch_omits_unused_camera_and_all_its_transport(tmp_path, monkeypatch, renamed_views): + config = load_product_config(PACKAGE / "config/o6_right_product.yaml", + workspace=PACKAGE.parents[1], check_can=False) + namespace = config.calibration_contract.typed_profile.namespace + arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", + record_bag=True, commands_enabled=True, diagnostic_capture="o6_thumb")[4:])) + rename = {"front": "front", "side": "side", "top": "top"} + if renamed_views: + rename = {"front": "inspection_a", "side": "inspection_b", "top": "inspection_c"} + payload = yaml.safe_load(config.profile_config.read_text()) + for view in payload["vision"]["views"]: + view["name"] = rename[view["name"]] + payload["vision"]["extrinsic_reference_view"] = rename[payload["vision"]["extrinsic_reference_view"]] + for task in payload["motion"]["tasks"]: + task["view"] = rename[task["view"]] + for measurement in payload["measurement"]["measurements"].values(): + measurement["view"] = rename[measurement["view"]] + path = tmp_path / "renamed.yaml" + path.write_text(yaml.safe_dump(payload)) + arguments["profile_config"] = str(path) + arguments["profile_config_expected_sha256"] = hashlib.sha256(path.read_bytes()).hexdigest() + for old, new in rename.items(): + for suffix in ("camera_serial", "camera_name", "camera_info_url"): + arguments[f"{new}_{suffix}"] = arguments.pop(f"{old}_{suffix}") + + # No side-camera launch arguments are supplied. A disconnected unused + # camera must not produce a driver process that could fail this session. + for suffix in ("camera_serial", "camera_name", "camera_info_url"): + arguments.pop(f"{rename['side']}_{suffix}") + context = LaunchContext() + context.launch_configurations.update(arguments) + module = launch_module() + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + + components = [] + def capture_container(**kwargs): + components.extend(kwargs["composable_node_descriptions"]) + return ComposableNodeContainer(**kwargs) + monkeypatch.setattr(module, "ComposableNodeContainer", capture_container) + actions = module._launch_stack(context) + nodes = [action for action in actions if isinstance(action, Node)] + assert len(nodes) == 6 # Two cameras, vision, border filter, SDK, coordinator. + assert len(components) == 4 # Rectification and detection for each active view. + component_namespaces = {perform_substitutions(context, component.node_namespace) + for component in components} + assert component_namespaces == {f"{namespace}/{view}/{kind}" + for view in (rename["front"], rename["top"]) + for kind in ("camera", "apriltag")} + + bag, = [action for action in actions + if isinstance(action, ExecuteProcess) and not isinstance(action, Node)] + bag_command = [perform_substitutions(context, part) for part in bag.cmd] + assert all(f"/{rename['side']}/" not in argument for argument in bag_command) + for view in (rename["front"], rename["top"]): + assert f"{namespace}/{view}/camera/image_raw" in bag_command + assert f"{namespace}/{view}/camera/camera_info" in bag_command + assert f"{namespace}/{view}/apriltag/detections" in bag_command + assert not (tmp_path / "session/raw_samples.jsonl").exists() diff --git a/src/linkerhand_calibration/test/test_unified_ros_host.py b/src/linkerhand_calibration/test/test_unified_ros_host.py index bf5aa75..b107624 100644 --- a/src/linkerhand_calibration/test/test_unified_ros_host.py +++ b/src/linkerhand_calibration/test/test_unified_ros_host.py @@ -6,8 +6,10 @@ import numpy as np import pytest -@pytest.mark.parametrize("model", ["l6", "o6", "g20", "o12"]) -def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, model): +@pytest.mark.parametrize("model,diagnostic_capture", [ + ("l6", ""), ("o6", ""), ("g20", ""), ("o12", ""), ("o6", "o6_thumb"), +]) +def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, model, diagnostic_capture): rclpy = pytest.importorskip("rclpy") from sensor_msgs.msg import JointState from std_srvs.srv import Trigger @@ -21,6 +23,8 @@ def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, m params = {"serial_number": "OFFLINE_HOST", "session_dir": str(tmp_path / model), "source_urdf_path": str(config.source_urdf), "camera_extrinsics_file": str(config.camera_extrinsics), **{key.replace("_sha256", "_expected_sha256"): value for key, value in protected_inputs(config).items()}} + if diagnostic_capture: + params["diagnostic_capture"] = diagnostic_capture args = ["--ros-args", "--params-file", str(config.calibration_config)] for key, value in params.items(): args += ["-p", f"{key}:={value}"] @@ -32,6 +36,18 @@ def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, m try: node = UnifiedCalibrationNode(profile) host = node.coordinator + views = ("front", "top") if diagnostic_capture else profile.vision.view_names + assert all(subscription.callback_group is node.default_callback_group + for subscription in node.subscriptions) + subscriptions = {subscription.topic_name for subscription in node.subscriptions} + expected_state_topic = host.parameters.state_topic + ("_timed" if profile.sdk_adapter == "legacy_byte_sdk" else "") + assert expected_state_topic in subscriptions + if profile.sdk_adapter == "legacy_byte_sdk": + assert host.parameters.state_topic not in subscriptions + for view in profile.vision.view_names: + assert (f"{profile.namespace}/{view}/camera/camera_info" in subscriptions) == (view in views) + assert f"{profile.namespace}/{view}/camera/image_rect" not in subscriptions + assert (f"{profile.namespace}/{view}/apriltag/detections_checked" in subscriptions) == (view in views) assert host.parameters.tracking.reprojection_tie_px == 0.03 assert all(tracker.reprojection_tie_px == 0.03 for tracker in host.capture.trackers.values()) if profile.sdk_adapter == "o12_hcan_sdk": @@ -43,10 +59,22 @@ def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, m message.position = list(map(float, profile.command.baseline_values)) message.name = list(profile.command.names) message.header.stamp = node.get_clock().now().to_msg() - node._state_callback(message) - host.cameras.matrices = {view: np.eye(3) for view in profile.vision.view_names} + def feed_message(): + if profile.sdk_adapter == "legacy_byte_sdk": + import json + from std_msgs.msg import String + stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec + timed = String() + timed.data = json.dumps(dict(schema="can_feedback_v1", name=list(message.name), + position=list(message.position), stamp_ns=stamp, + channel_stamps_ns=[stamp]*len(message.position))) + node._timed_state_callback(timed) + else: + node._state_callback(message) + feed_message() + host.cameras.matrices = {view: np.eye(3) for view in views} import time - host.cameras.info_received_at = {view: time.monotonic() for view in profile.vision.view_names} + host.cameras.info_received_at = {view: time.monotonic() for view in views} host.cameras.detections_received_at = dict(host.cameras.info_received_at) node._tick() assert host.execution.session.phase == Phase.READY @@ -62,7 +90,7 @@ def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, m assert host.execution.session.phase == Phase.WAIT_DEVICE assert not writes assert host.capture is preview_capture - host.cameras.detections_received_at = {view: time.monotonic() for view in profile.vision.view_names} + host.cameras.detections_received_at = {view: time.monotonic() for view in views} node._tick() response = node._start(Trigger.Request(), Trigger.Response()) assert response.success @@ -73,11 +101,10 @@ def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, m node._tick() assert not writes # Still waits for a fresh, post-Start feedback frame. message.header.stamp = node.get_clock().now().to_msg() - node._state_callback(message) + feed_message() node._tick() assert host.execution.session.phase == Phase.BASELINE finally: if node is not None: node.destroy_node() rclpy.shutdown() - diff --git a/src/linkerhand_calibration/test/test_unified_runner.py b/src/linkerhand_calibration/test/test_unified_runner.py index 471bb77..d965adb 100644 --- a/src/linkerhand_calibration/test/test_unified_runner.py +++ b/src/linkerhand_calibration/test/test_unified_runner.py @@ -8,9 +8,10 @@ from types import SimpleNamespace import pytest from linkerhand_calibration.profiles import load_hand_profile +from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION from linkerhand_calibration.product import load_product_config from linkerhand_calibration.runtime import runner -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION from linkerhand_calibration.runtime.resume import discover_resume_candidate from linkerhand_calibration.runtime.status import normalize_status, render_status_zh @@ -142,6 +143,8 @@ def _journal(path, *, version=ACQUISITION_POLICY_VERSION, digest="a" * 64, compl path.mkdir() rows = [{"kind": "session_start", "profile_id": "VIRTUAL/right/layout/v1", "serial_number": "TEST_001", "acquisition_policy_version": version, + "capture_schedule_version": CAPTURE_SCHEDULE_VERSION, + "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, "protected_hashes": {"source": digest}}, {"kind": "fixed_base_reference_locked"}] if complete: diff --git a/src/linkerhand_calibration/test/test_zero_recovery.py b/src/linkerhand_calibration/test/test_zero_recovery.py new file mode 100644 index 0000000..902bf48 --- /dev/null +++ b/src/linkerhand_calibration/test/test_zero_recovery.py @@ -0,0 +1,72 @@ +"""Bounded fresh preparation, no replacement of already frozen joint zeros.""" + +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution +from linkerhand_calibration.runtime.branch_initialization import solve_initialization +from linkerhand_calibration.runtime.zero_recovery import ZeroRecovery +from runtime_host_fixture import coordinator_fixture, ready +from test_diagnostic_coordinator import run_until + + +def test_recovery_is_bounded_and_rejects_frozen_or_faulty_inputs(): + motion = SimpleNamespace(task_key="chain", zero_joints=("a", "b")) + policy = ZeroRecovery() + reports = {"front": {"status": "unresolved", "reason": "image_motion_families_not_distinguishable"}} + kwargs = dict(geometry_error="ambiguous", reports=reports, referenced_joints={}) + assert policy.take(motion, **{**kwargs, "referenced_joints": {"a": object()}}) is None + assert policy.take(motion, **{**kwargs, "reports": {"front": {"status": "unresolved", "reason": "motion_held_feedback_changed"}}}) is None + assert policy.take(motion, **kwargs) == "repeat_approach" + assert policy.take(motion, **kwargs) is None + assert policy.take(motion, geometry_error="", reports={}, referenced_joints={}) is None + + +@pytest.mark.parametrize("persistent", [False, True]) +def test_transient_preparation_retries_locally_and_persistent_failure_stops(tmp_path, persistent): + host, clock = coordinator_fixture(tmp_path) + ready(host, clock) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + requests = [] + def solver(request): + requests.append(request) + if persistent or len(requests) == 1: + return ImageMotionResolution(False, "image_motion_insufficient_independent_frames") + return solve_initialization(request) + host.initialization_solver = solver + assert host.start().success + try: + run_until(host, clock, lambda: host.state == "PAUSED" or bool(host.execution.zero_references)) + assert len(requests) == 2 + assert max(frame.stamp_ns for frame in requests[0].frames) < min(frame.stamp_ns for frame in requests[1].frames) + assert len([r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"]) == 1 + if persistent: + assert host.state == "PAUSED" and not host.execution.zero_references + else: + assert host.state == "RUNNING" and "rh_thumb_cmc_yaw" in host.execution.zero_references + assert not host.finalization.started + finally: + host.close() + + +def test_zero_image_recovery_keeps_frozen_geometry_and_pose(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + ready(host, clock) + for view in host.required_observation_views: + host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]]) + original = host._lock_joint_zeros + def delay_images(motion): + recovered = any(r.get("kind") == "joint_zero_recovery" for r in host.raw_records) + return original(motion) if recovered else False + monkeypatch.setattr(host, "_lock_joint_zeros", delay_images) + assert host.start().success + try: + run_until(host, clock, lambda: bool(host.execution.zero_references)) + recoveries = [r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"] + assert len(recoveries) == 1 and recoveries[0]["action"] == "retry_zero_images" + assert len([r for r in host.raw_records if r.get("kind") == "motion_branch_initialization"]) == 1 + finally: + host.close()