ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

4b_revolute mit Fallback

Browse Source
This commit is contained in:
chk
2026-06-16 14:52:18 +02:00
parent ad208b7d21
commit 0234c1ef1d
17 changed files with 14069 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
scripts/4b_revolute_angle.py
View File

@@ -5,11 +5,17 @@
Generic revolute-joint angle estimator. Generic revolute-joint angle estimator.
For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute', For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute',
this script estimates the rotation angle using the pairwise-vector method: this script estimates the rotation angle using the pairwise-vector method
(PRIMARY), with a single-marker pivot method as FALLBACK:
For every PAIR (m1, m2) of markers belonging to the target link: PRIMARY — for every PAIR (m1, m2) of markers belonging to the target link:
v_model = local_pos_m2 - local_pos_m1 (in link's own frame) v_model = spoke_world(m2) - spoke_world(m1) (model, world-oriented)
v_obs = world_pos_m2 - world_pos_m1 (in world frame) v_obs = world_pos_m2 - world_pos_m1 (observed, world frame)
where spoke_world(m) rotates the marker's local `position` (from
robot.json) through the already-known PARENT joints via FK, so it is
expressed in world orientation at this joint's own angle = 0 — exactly
the frame v_obs lives in.
Both vectors are projected perpendicular to the joint axis (in world frame), Both vectors are projected perpendicular to the joint axis (in world frame),
and the signed angle from v_model_perp to v_obs_perp is measured. and the signed angle from v_model_perp to v_obs_perp is measured.
@@ -20,6 +26,16 @@ this script estimates the rotation angle using the pairwise-vector method:
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable). Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
FALLBACK — only used when the PRIMARY method has no usable pair at all
(e.g. just one marker visible, or every visible pair happens to lie
parallel to the joint axis, as for two markers spaced along a forearm):
the joint PIVOT itself stands in for the missing second marker, i.e. the
"pair" becomes (pivot, m1). This needs only ONE matched marker, but —
unlike the primary method — its accuracy additionally depends on the
already-estimated PARENT joint *values* being correct (not just their
axis direction), since the pivot's world position comes from FK. See
`PIVOT_FALLBACK_ID` / `used_fallback` in the code.
How to use sequentially How to use sequentially
----------------------- -----------------------
Run 4b once per revolute joint, from root to tip: Run 4b once per revolute joint, from root to tip:
@@ -36,6 +52,7 @@ Output JSON
{ {
"link": "Arm1", "link": "Arm1",
"joint": "y", "joint": "y",
"method": "marker_pair", // or "pivot_fallback" — see FALLBACK above
"mean_angle_deg": 86.3, "mean_angle_deg": 86.3,
"circular_std_deg": 0.7, "circular_std_deg": 0.7,
"num_pairs": 6, "num_pairs": 6,
@@ -62,6 +79,12 @@ from robot_fk import RobotFK
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e") STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
# Sentinel "marker id" used in `per_pair` reports for the joint pivot.
# Only ever appears when the FALLBACK path (pivot vs. a single marker)
# was used instead of a real marker-to-marker pair — see the
# `used_fallback` block inside `estimate_revolute_angle()` below.
PIVOT_FALLBACK_ID = -1
# ────────────────────────────────────────────────────────────── # ──────────────────────────────────────────────────────────────
# I/O # I/O
@@ -130,6 +153,78 @@ def _circular_mean_deg(angles_rad: np.ndarray,
return math.degrees(mean), c_var, math.degrees(c_std) return math.degrees(mean), c_var, math.degrees(c_std)
# ──────────────────────────────────────────────────────────────
# Shared spoke/pair math
# (used by BOTH the primary marker-pair method and the pivot FALLBACK)
# ──────────────────────────────────────────────────────────────
def _model_spoke_world(fk: RobotFK,
zero_transforms: Dict[str, np.ndarray],
link_name: str,
origin_world: np.ndarray,
local_pos: np.ndarray) -> np.ndarray:
"""
Vector from the joint PIVOT to a marker, in WORLD ORIENTATION, as it
would be if this joint's own angle were 0 (only the already-known
PARENT rotations applied).
This is what `v_model` must be expressed as before comparing it to
`axis_world` / an observed vector: the raw `position` field from
robot.json lives in the link's own pre-rotation local frame and is
NOT yet rotated into world orientation by the parent chain. Skipping
this rotation silently biases the estimated angle by (roughly) the
parent joint's own angle — invisible whenever the parent chain has
no rotation yet (e.g. Arm1, whose parent 'Base' is purely linear),
but wrong for anything further down the chain (Ellbow, Arm2, Hand …).
"""
p0 = fk.marker_world(zero_transforms, link_name, local_pos)
return p0 - origin_world
def _pair_estimate(v_model: np.ndarray,
v_obs: np.ndarray,
axis_world: np.ndarray,
marker_ids: Tuple[int, int],
min_baseline_mm: float,
fallback: bool) -> Tuple[Optional[float], Optional[float], dict]:
"""
Project model/observed vectors perpendicular to the joint axis and
derive one angle estimate from them. Returns (angle_rad, weight,
per_pair_entry) — angle_rad/weight are None when skipped (baseline
too short).
`fallback=True` marks entries produced by the pivot FALLBACK (one of
the two "markers" is actually the joint pivot, see PIVOT_FALLBACK_ID)
so callers/reports can always tell primary and fallback data apart.
"""
v_model_perp = _project_perp(v_model, axis_world)
v_obs_perp = _project_perp(v_obs, axis_world)
bl_model = float(np.linalg.norm(v_model_perp))
bl_obs = float(np.linalg.norm(v_obs_perp))
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
return None, None, {
"marker_ids": list(marker_ids),
"fallback": fallback,
"skipped": True,
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
}
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
weight = bl_model * bl_obs
entry = {
"marker_ids": list(marker_ids),
"fallback": fallback,
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": weight,
}
return angle, weight, entry
# ────────────────────────────────────────────────────────────── # ──────────────────────────────────────────────────────────────
# Core estimator (generic — works for any revolute joint) # Core estimator (generic — works for any revolute joint)
# ────────────────────────────────────────────────────────────── # ──────────────────────────────────────────────────────────────
@@ -179,8 +274,9 @@ def estimate_revolute_angle(
zero_state = dict(known_state) zero_state = dict(known_state)
zero_state[var] = 0.0 zero_state[var] = 0.0
origin_world = fk.joint_origin_world(link_name, zero_state) origin_world = fk.joint_origin_world(link_name, zero_state)
axis_world = fk.joint_axis_world(link_name, zero_state) axis_world = fk.joint_axis_world(link_name, zero_state)
zero_transforms = fk.compute(zero_state) # needed to rotate model spokes into world orientation
# ── collect matched markers ─────────────────────────────── # ── collect matched markers ───────────────────────────────
model_local: Dict[int, np.ndarray] = {} model_local: Dict[int, np.ndarray] = {}
@@ -192,15 +288,24 @@ def estimate_revolute_angle(
matched = {mid: (model_local[mid], observed_mm[mid]) matched = {mid: (model_local[mid], observed_mm[mid])
for mid in model_local if mid in observed_mm} for mid in model_local if mid in observed_mm}
if len(matched) < 2: # Only 1 matched marker is enough to *attempt* an estimate — the
# PIVOT FALLBACK below can work with a single marker. With 0 there
# is nothing to go on at all.
if len(matched) < 1:
return { return {
"status": "failed", "status": "failed",
"reason": (f"Need ≥2 matched markers, found {len(matched)}: " "reason": (f"Need ≥1 matched marker, found {len(matched)}. "
f"{list(matched.keys())}. "
f"Model marker IDs: {list(model_local.keys())}"), f"Model marker IDs: {list(model_local.keys())}"),
} }
# ── pairwise estimation ─────────────────────────────────── def _spoke(local_pos: np.ndarray) -> np.ndarray:
return _model_spoke_world(fk, zero_transforms, link_name, origin_world, local_pos)
# ── PRIMARY: marker-to-marker pairs within this link ──────
# Preferred whenever ≥2 markers with a usable (non axis-parallel)
# baseline are visible. Only the AXIS DIRECTION needs to be correct
# for this — not the pivot's position — so it is the more robust
# source of truth and is always tried first.
ids = sorted(matched.keys()) ids = sorted(matched.keys())
angle_rad_list: List[float] = [] angle_rad_list: List[float] = []
weight_list: List[float] = [] weight_list: List[float] = []
@@ -210,42 +315,49 @@ def estimate_revolute_angle(
l1, o1 = matched[id1] l1, o1 = matched[id1]
l2, o2 = matched[id2] l2, o2 = matched[id2]
v_model = l2 - l1 # local frame, both in same link v_model = _spoke(l2) - _spoke(l1) # model, world-oriented
v_obs = o2 - o1 # world frame v_obs = o2 - o1 # observed, world frame
v_model_perp = _project_perp(v_model, axis_world) angle, weight, entry = _pair_estimate(
v_obs_perp = _project_perp(v_obs, axis_world) v_model, v_obs, axis_world, (id1, id2), min_baseline_mm, fallback=False)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
bl_model = float(np.linalg.norm(v_model_perp)) # ── FALLBACK: pivot + single marker, axis from predecessor ────
bl_obs = float(np.linalg.norm(v_obs_perp)) # Only entered when the PRIMARY method above produced NOT A SINGLE
# usable pair (e.g. only one marker visible at all, or every visible
# pair happens to lie parallel to the joint axis — as for two
# markers spaced along a forearm). Each matched marker is paired
# with the joint PIVOT instead of another marker, using the
# rotation axis already known from the predecessor joints.
# This is strictly a fallback: compared to a real 2-marker baseline
# it additionally relies on the predecessor joints' *values* (not
# just their axis direction) being accurate, since the pivot's
# world position is computed via FK rather than observed directly.
used_fallback = False
if not angle_rad_list:
used_fallback = True
for mid in ids:
l, o = matched[mid]
v_model = _spoke(l) # pivot → marker, model, world-oriented
v_obs = o - origin_world # pivot → marker, observed
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm: angle, weight, entry = _pair_estimate(
per_pair.append({ v_model, v_obs, axis_world,
"marker_ids": [id1, id2], (PIVOT_FALLBACK_ID, mid), min_baseline_mm, fallback=True)
"skipped": True, per_pair.append(entry)
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}", if angle is not None:
}) angle_rad_list.append(angle)
continue weight_list.append(weight)
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
w = bl_model * bl_obs
angle_rad_list.append(angle)
weight_list.append(w)
per_pair.append({
"marker_ids": [id1, id2],
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": w,
})
if not angle_rad_list: if not angle_rad_list:
return { return {
"status": "failed", "status": "failed",
"reason": "All pairs below min_baseline_mm. " "reason": "All pairs below min_baseline_mm, including the "
"Try --min-baseline 5 or check step-3 output.", "pivot fallback. Try --min-baseline 5 or check "
"step-3 output.",
} }
mean_deg, c_var, c_std_deg = _circular_mean_deg( mean_deg, c_var, c_std_deg = _circular_mean_deg(
@@ -258,16 +370,23 @@ def estimate_revolute_angle(
print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm") print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm")
print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]") print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]")
print(f" Matched markers: {list(matched.keys())}") print(f" Matched markers: {list(matched.keys())}")
print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}") if used_fallback:
print(f" [FALLBACK] No usable marker-marker pair — estimating from "
f"pivot + predecessor axis instead (single-marker spokes).")
print(f" Pairs used: {len(angle_rad_list)} / {len(per_pair)}")
print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °") print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °")
if c_std_deg > 5.0: if c_std_deg > 5.0:
print(f" [WARN] high spread – step-3 errors or marker overlap") print(f" [WARN] high spread – step-3 errors or marker overlap")
print(f"\n Pair detail:") print(f"\n Pair detail:")
for pp in per_pair: for pp in per_pair:
id0, id1_ = pp["marker_ids"]
m0 = "PIVOT" if id0 == PIVOT_FALLBACK_ID else f"M{id0}"
m1 = "PIVOT" if id1_ == PIVOT_FALLBACK_ID else f"M{id1_}"
tag = " [fallback]" if pp.get("fallback") else ""
if pp["skipped"]: if pp["skipped"]:
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}") print(f" {m0}↔{m1}{tag}: SKIPPED – {pp['reason']}")
else: else:
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: " print(f" {m0}↔{m1}{tag}: "
f"{pp['angle_deg']:+7.2f}° " f"{pp['angle_deg']:+7.2f}° "
f"bl_model={pp['baseline_model_mm']:.1f} " f"bl_model={pp['baseline_model_mm']:.1f} "
f"bl_obs={pp['baseline_obs_mm']:.1f}") f"bl_obs={pp['baseline_obs_mm']:.1f}")
@@ -280,6 +399,7 @@ def estimate_revolute_angle(
"status": "ok", "status": "ok",
"link": link_name, "link": link_name,
"joint": var, "joint": var,
"method": "pivot_fallback" if used_fallback else "marker_pair",
"joint_origin_world_mm": origin_world.tolist(), "joint_origin_world_mm": origin_world.tolist(),
"joint_axis_world": axis_world.tolist(), "joint_axis_world": axis_world.tolist(),
"mean_angle_deg": mean_deg, "mean_angle_deg": mean_deg,

BIN
scripts/__pycache__/robot_fk.cpython-311.pyc Normal file
View File

Binary file not shown.

46
test/homing/20260616_120456/aruco_marker_poses.csv Normal file
View File

@@ -0,0 +1,46 @@
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
47,Board,A0,3,343.56,-286.5,-27.02,0.01282,0.04293,0.999,344.23,-286.54,-27.3,0.727,0.278,23.86
51,Board,A0,2,168.06,-172.05,-26.96,-0.06575,-0.00903,0.99779,167.8,-172.08,-27.3,0.432,0.344,23.62
54,Board,A0,3,341.85,-330.17,-26.58,0.02766,0.00621,0.9996,342.27,-330.59,-27.3,0.928,0.716,23.56
55,Board,A0,3,282.99,-262.34,-26.34,0.0153,0.00966,0.99984,283.72,-262.58,-27.3,1.231,0.961,23.68
57,Board,A0,2,602.73,-364.49,-27.92,0.02948,0.01103,0.9995,602.86,-364.05,-27.3,0.774,-0.622,23.95
62,Board,A0,3,403.64,-174.91,-26.43,0.02093,-0.01927,0.9996,404.7,-175.1,-27.3,1.381,0.868,23.73
65,Board,A0,2,805.42,-298.68,-28.85,0.07317,-0.00718,0.99729,803.39,-297.37,-27.3,2.872,-1.549,24.67
66,Board,A0,2,209.91,-362.69,-27.32,0.01431,0.0008,0.9999,209.75,-363.23,-27.3,0.56,-0.021,24.07
70,Board,A0,2,598.23,299.66,-24.5,-0.01925,0.12918,0.99143,601.87,300.33,-27.3,4.638,2.798,24.16
71,Board,A0,2,750.95,-284.52,-27.87,0.02144,-0.00558,0.99975,749.75,-284.01,-27.3,1.423,-0.571,24.07
79,Board,A0,2,312.62,-159.33,-27.38,-0.03005,-0.02443,0.99925,312.3,-159.11,-27.3,0.397,-0.077,23.93
80,Board,A0,2,866.21,-337.3,-29.23,0.03205,-0.01144,0.99942,863.59,-335.92,-27.3,3.54,-1.934,23.73
82,Board,A0,2,217.92,296.91,-26.35,-0.02521,0.05118,0.99837,219.16,297.24,-27.3,1.599,0.954,23.75
84,Board,A0,3,405.0,257.84,-25.4,0.01076,0.0148,0.99983,407.49,258.42,-27.3,3.183,1.897,23.57
85,Board,A0,3,504.05,-312.91,-27.97,-0.00509,-0.01089,0.99993,504.58,-312.75,-27.3,0.869,-0.668,23.76
86,Board,A0,2,362.68,291.99,-27.28,-0.01296,0.03562,0.99928,362.89,292.01,-27.3,0.209,0.022,23.91
87,Board,A0,2,946.89,-246.84,-29.25,0.03555,0.02881,0.99895,943.63,-245.76,-27.3,3.952,-1.95,24.15
90,Board,A0,2,639.54,315.8,-24.64,-0.00908,0.0411,0.99911,643.17,316.43,-27.3,4.55,2.665,23.68
91,Board,A0,2,719.12,327.68,-24.61,-0.07043,0.08617,0.99379,723.35,328.05,-27.3,5.028,2.687,23.57
93,Board,A0,2,932.27,143.24,-24.03,-0.05724,0.02723,0.99799,934.88,143.6,-27.3,4.202,3.272,22.76
95,Board,A0,2,186.15,-273.72,-26.9,0.01775,-0.00563,0.99983,186.04,-274.07,-27.3,0.548,0.404,23.8
96,Board,A0,3,368.95,-186.48,-26.8,-0.02679,0.03068,0.99917,369.77,-186.49,-27.3,0.961,0.499,23.46
97,Board,A0,2,304.05,-359.12,-27.02,0.00663,-0.01188,0.99991,304.35,-359.67,-27.3,0.689,0.284,23.61
99,Board,A0,2,963.23,-323.27,-29.97,0.05209,-0.03007,0.99819,959.16,-321.55,-27.3,5.165,-2.671,23.74
104,Board,A0,2,822.76,238.49,-24.06,-0.01937,0.04301,0.99889,826.71,239.16,-27.3,5.157,3.245,23.49
105,Board,A0,3,524.14,-266.3,-27.48,0.04369,0.05496,0.99753,524.84,-266.25,-27.3,0.728,-0.183,24.08
129,Ellbow,,2,392.44,-57.59,273.3,-0.02284,-0.02831,0.99934,,,,,,23.22
132,Ellbow,,2,355.65,-57.41,273.66,0.00682,-0.01043,0.99992,,,,,,23.57
143,Arm2,,3,368.17,-239.23,261.07,-0.6936,-0.02269,0.72001,,,,,,24.16
144,Arm2,,3,356.52,-168.54,237.95,-0.99956,0.02529,0.01551,,,,,,24.28
146,Arm2,,2,363.47,-167.73,265.11,-0.69227,-0.01571,0.72147,,,,,,23.95
147,unknown,,2,361.52,-238.42,219.07,-0.87822,0.02426,-0.47764,,,,,,25.11
148,Arm2,,3,356.84,-275.1,235.17,-0.99971,0.02422,-0.00165,,,,,,24.49
197,Arm1,,2,253.16,-55.28,243.87,-0.99795,-0.06394,0.00227,,,,,,24.78
200,unknown,,2,231.14,-30.63,109.67,-0.03466,0.04681,0.9983,,,,,,22.96
201,unknown,,3,195.05,46.33,96.05,-0.99847,-0.02285,0.05033,,,,,,24.23
204,unknown,,3,230.13,118.96,116.92,-0.00845,0.01273,0.99988,,,,,,23.55
205,unknown,,2,935.95,-97.07,-6.58,-0.01249,0.09996,0.99491,,,,,,23.18
215,unknown,,2,334.82,-96.93,-7.71,-0.02232,0.05948,0.99798,,,,,,23.34
243,Arm1,,2,303.21,-79.66,265.92,0.02387,-0.65211,0.75775,,,,,,23.8
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
cam0,-352.63,-78.05,407.29,0.94846,0.07547,-0.30777
cam1,-232.55,-598.43,854.78,0.51862,0.43559,-0.73573
cam2,-237.23,81.29,837.43,0.70662,-0.07011,-0.70411
1 marker_id link set num_cameras x_mm y_mm z_mm nx ny nz model_x_mm model_y_mm model_z_mm dist_to_model_mm delta_z_mm edge_length_mm
2 47 Board A0 3 343.56 -286.5 -27.02 0.01282 0.04293 0.999 344.23 -286.54 -27.3 0.727 0.278 23.86
3 51 Board A0 2 168.06 -172.05 -26.96 -0.06575 -0.00903 0.99779 167.8 -172.08 -27.3 0.432 0.344 23.62
4 54 Board A0 3 341.85 -330.17 -26.58 0.02766 0.00621 0.9996 342.27 -330.59 -27.3 0.928 0.716 23.56
5 55 Board A0 3 282.99 -262.34 -26.34 0.0153 0.00966 0.99984 283.72 -262.58 -27.3 1.231 0.961 23.68
6 57 Board A0 2 602.73 -364.49 -27.92 0.02948 0.01103 0.9995 602.86 -364.05 -27.3 0.774 -0.622 23.95
7 62 Board A0 3 403.64 -174.91 -26.43 0.02093 -0.01927 0.9996 404.7 -175.1 -27.3 1.381 0.868 23.73
8 65 Board A0 2 805.42 -298.68 -28.85 0.07317 -0.00718 0.99729 803.39 -297.37 -27.3 2.872 -1.549 24.67
9 66 Board A0 2 209.91 -362.69 -27.32 0.01431 0.0008 0.9999 209.75 -363.23 -27.3 0.56 -0.021 24.07
10 70 Board A0 2 598.23 299.66 -24.5 -0.01925 0.12918 0.99143 601.87 300.33 -27.3 4.638 2.798 24.16
11 71 Board A0 2 750.95 -284.52 -27.87 0.02144 -0.00558 0.99975 749.75 -284.01 -27.3 1.423 -0.571 24.07
12 79 Board A0 2 312.62 -159.33 -27.38 -0.03005 -0.02443 0.99925 312.3 -159.11 -27.3 0.397 -0.077 23.93
13 80 Board A0 2 866.21 -337.3 -29.23 0.03205 -0.01144 0.99942 863.59 -335.92 -27.3 3.54 -1.934 23.73
14 82 Board A0 2 217.92 296.91 -26.35 -0.02521 0.05118 0.99837 219.16 297.24 -27.3 1.599 0.954 23.75
15 84 Board A0 3 405.0 257.84 -25.4 0.01076 0.0148 0.99983 407.49 258.42 -27.3 3.183 1.897 23.57
16 85 Board A0 3 504.05 -312.91 -27.97 -0.00509 -0.01089 0.99993 504.58 -312.75 -27.3 0.869 -0.668 23.76
17 86 Board A0 2 362.68 291.99 -27.28 -0.01296 0.03562 0.99928 362.89 292.01 -27.3 0.209 0.022 23.91
18 87 Board A0 2 946.89 -246.84 -29.25 0.03555 0.02881 0.99895 943.63 -245.76 -27.3 3.952 -1.95 24.15
19 90 Board A0 2 639.54 315.8 -24.64 -0.00908 0.0411 0.99911 643.17 316.43 -27.3 4.55 2.665 23.68
20 91 Board A0 2 719.12 327.68 -24.61 -0.07043 0.08617 0.99379 723.35 328.05 -27.3 5.028 2.687 23.57
21 93 Board A0 2 932.27 143.24 -24.03 -0.05724 0.02723 0.99799 934.88 143.6 -27.3 4.202 3.272 22.76
22 95 Board A0 2 186.15 -273.72 -26.9 0.01775 -0.00563 0.99983 186.04 -274.07 -27.3 0.548 0.404 23.8
23 96 Board A0 3 368.95 -186.48 -26.8 -0.02679 0.03068 0.99917 369.77 -186.49 -27.3 0.961 0.499 23.46
24 97 Board A0 2 304.05 -359.12 -27.02 0.00663 -0.01188 0.99991 304.35 -359.67 -27.3 0.689 0.284 23.61
25 99 Board A0 2 963.23 -323.27 -29.97 0.05209 -0.03007 0.99819 959.16 -321.55 -27.3 5.165 -2.671 23.74
26 104 Board A0 2 822.76 238.49 -24.06 -0.01937 0.04301 0.99889 826.71 239.16 -27.3 5.157 3.245 23.49
27 105 Board A0 3 524.14 -266.3 -27.48 0.04369 0.05496 0.99753 524.84 -266.25 -27.3 0.728 -0.183 24.08
28 129 Ellbow 2 392.44 -57.59 273.3 -0.02284 -0.02831 0.99934 23.22
29 132 Ellbow 2 355.65 -57.41 273.66 0.00682 -0.01043 0.99992 23.57
30 143 Arm2 3 368.17 -239.23 261.07 -0.6936 -0.02269 0.72001 24.16
31 144 Arm2 3 356.52 -168.54 237.95 -0.99956 0.02529 0.01551 24.28
32 146 Arm2 2 363.47 -167.73 265.11 -0.69227 -0.01571 0.72147 23.95
33 147 unknown 2 361.52 -238.42 219.07 -0.87822 0.02426 -0.47764 25.11
34 148 Arm2 3 356.84 -275.1 235.17 -0.99971 0.02422 -0.00165 24.49
35 197 Arm1 2 253.16 -55.28 243.87 -0.99795 -0.06394 0.00227 24.78
36 200 unknown 2 231.14 -30.63 109.67 -0.03466 0.04681 0.9983 22.96
37 201 unknown 3 195.05 46.33 96.05 -0.99847 -0.02285 0.05033 24.23
38 204 unknown 3 230.13 118.96 116.92 -0.00845 0.01273 0.99988 23.55
39 205 unknown 2 935.95 -97.07 -6.58 -0.01249 0.09996 0.99491 23.18
40 215 unknown 2 334.82 -96.93 -7.71 -0.02232 0.05948 0.99798 23.34
41 243 Arm1 2 303.21 -79.66 265.92 0.02387 -0.65211 0.75775 23.8
42 camera_id x_mm y_mm z_mm dir_x dir_y dir_z
43 cam0 -352.63 -78.05 407.29 0.94846 0.07547 -0.30777
44 cam1 -232.55 -598.43 854.78 0.51862 0.43559 -0.73573
45 cam2 -237.23 81.29 837.43 0.70662 -0.07011 -0.70411

1827
test/homing/20260616_120456/aruco_marker_poses.json Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
test/homing/20260616_120456/cam0.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 74 KiB

2376
test/homing/20260616_120456/cam0_aruco_detection.json Normal file
View File

File diff suppressed because it is too large Load Diff

411
test/homing/20260616_120456/cam0_camera_pose.json Normal file
View File

@@ -0,0 +1,411 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-16T12:05:00Z",
"source": {
"detection_json": "/app/data/homing/20260616_120456/cam0_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam0",
"camera_matrix": [
[
1424.7584228515625,
0.0,
635.95947265625
],
[
0.0,
1421.5770263671875,
482.1744384765625
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.05634751915931702,
0.33765655755996704,
0.002130246954038739,
-0.004022662527859211,
-1.182201862335205
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 15,
"used_marker_ids": [
55,
79,
47,
54,
86,
96,
84,
62,
85,
105,
57,
59,
48,
92,
71
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.00867338622090616,
0.0005736213711697816,
0.0003387058057489281,
0.00033870238542763406
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 0.6950564647542702,
"residual_median_px": 0.40248905261772255,
"residual_max_px": 1.4324357568626296,
"sigma2_normalized": 1.4339913236727992e-07
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
0.054954398423433304,
-0.995682954788208,
-0.07480252534151077
],
[
-0.31208696961402893,
0.05403408408164978,
-0.9485157132148743
],
[
0.9484628438949585,
0.07547000050544739,
-0.3077702820301056
]
],
"translation_m": [
-0.027871036902070045,
0.2804853320121765,
0.4656999111175537
],
"rvec_rad": [
1.4158355861217138,
-1.414839573060464,
0.9451885483658883
]
},
"camera_in_world": {
"position_m": [
-0.3526315987110138,
-0.07805286347866058,
0.40728849172592163
],
"position_mm": [
-352.631591796875,
-78.05286407470703,
407.2884826660156
],
"orientation_deg": {
"roll": 166.22206115722656,
"pitch": -71.525146484375,
"yaw": -80.01333618164062
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
5.024150406033972e-07,
-2.6272364342807006e-07,
-4.335547550274572e-08,
3.714638912704353e-08,
1.63462035841052e-07,
1.0652181985979162e-07
],
[
-2.627236434280717e-07,
5.551190890670724e-07,
-9.010350610641584e-08,
-1.9385507269862047e-07,
-1.980878719033606e-07,
-9.31153574198941e-08
],
[
-4.335547550274997e-08,
-9.01035061064162e-08,
6.53136070312492e-07,
1.9362202241313713e-07,
-1.146589237690379e-07,
-7.196585886255944e-08
],
[
3.714638912704331e-08,
-1.9385507269862087e-07,
1.9362202241313628e-07,
1.2484560913735146e-07,
3.2658382583721026e-08,
3.301097074312566e-08
],
[
1.6346203584105354e-07,
-1.980878719033603e-07,
-1.1465892376903774e-07,
3.265838258372065e-08,
1.23652189246973e-07,
8.571146080154154e-08
],
[
1.0652181985979293e-07,
-9.311535741989419e-08,
-7.196585886255985e-08,
3.301097074312538e-08,
8.571146080154191e-08,
1.6707738731885391e-07
]
],
"parameter_std": {
"rvec_std_deg": [
0.04061195988417994,
0.0426889736419385,
0.04630463517881947
],
"tvec_std_m": [
0.0003533349814798295,
0.000351642132354718,
0.00040875100895148127
]
},
"camera_center_std_m": [
0.0004015718950779406,
0.0007421256472201426,
0.0006731526149433535
],
"camera_center_std_mm": [
0.4015718950779406,
0.7421256472201425,
0.6731526149433535
],
"orientation_std_deg": {
"roll": 0.13494216320251604,
"pitch": 0.04025296957854174,
"yaw": 0.11737953488154021
}
}
},
"observations": {
"markers": [
{
"marker_id": 55,
"observed_center_px": [
1136.5,
888.25
],
"projected_center_px": [
1136.376220703125,
888.4700927734375
],
"reprojection_error_px": 0.25251166954079585,
"confidence": 0.5219323198445638
},
{
"marker_id": 79,
"observed_center_px": [
919.0,
861.25
],
"projected_center_px": [
919.0020141601562,
861.0948486328125
],
"reprojection_error_px": 0.15516444045362157,
"confidence": 0.46679997653940364
},
{
"marker_id": 47,
"observed_center_px": [
1151.25,
822.25
],
"projected_center_px": [
1150.4693603515625,
821.73828125
],
"reprojection_error_px": 0.9334100598419679,
"confidence": 0.40553614178530284
},
{
"marker_id": 54,
"observed_center_px": [
1235.25,
820.25
],
"projected_center_px": [
1236.1435546875,
821.3695678710938
],
"reprojection_error_px": 1.4324357568626296,
"confidence": 0.11853329759702871
},
{
"marker_id": 86,
"observed_center_px": [
123.0,
842.75
],
"projected_center_px": [
122.65746307373047,
842.6725463867188
],
"reprojection_error_px": 0.35118457834662914,
"confidence": 0.39666742757474877
},
{
"marker_id": 96,
"observed_center_px": [
954.0,
802.0
],
"projected_center_px": [
954.2156982421875,
801.8694458007812
],
"reprojection_error_px": 0.25213117739864394,
"confidence": 0.3314216578838723
},
{
"marker_id": 84,
"observed_center_px": [
206.75,
799.0
],
"projected_center_px": [
207.06143188476562,
799.0526123046875
],
"reprojection_error_px": 0.31584469831421874,
"confidence": 0.29360621858836283
},
{
"marker_id": 62,
"observed_center_px": [
925.0,
770.75
],
"projected_center_px": [
925.062255859375,
771.1116943359375
],
"reprojection_error_px": 0.36701305790910066,
"confidence": 0.25370304094044416
},
{
"marker_id": 85,
"observed_center_px": [
1120.5,
687.5
],
"projected_center_px": [
1119.8026123046875,
686.31298828125
],
"reprojection_error_px": 1.3767158087358151,
"confidence": 0.20441637110846192
},
{
"marker_id": 105,
"observed_center_px": [
1038.75,
675.5
],
"projected_center_px": [
1039.1170654296875,
675.0737915039062
],
"reprojection_error_px": 0.5624861881096861,
"confidence": 0.17259443971051047
},
{
"marker_id": 57,
"observed_center_px": [
1157.25,
621.75
],
"projected_center_px": [
1157.047607421875,
621.402099609375
],
"reprojection_error_px": 0.40248905261772255,
"confidence": 0.15036126370334021
},
{
"marker_id": 59,
"observed_center_px": [
1027.0,
613.25
],
"projected_center_px": [
1026.6114501953125,
613.2049560546875
],
"reprojection_error_px": 0.3911520263682778,
"confidence": 0.11849981411449093
},
{
"marker_id": 48,
"observed_center_px": [
1066.0,
578.25
],
"projected_center_px": [
1065.6326904296875,
578.4794921875
],
"reprojection_error_px": 0.433108513616032,
"confidence": 0.08787391972076787
},
{
"marker_id": 92,
"observed_center_px": [
892.75,
608.25
],
"projected_center_px": [
893.5529174804688,
608.5789794921875
],
"reprojection_error_px": 0.8677004014763569,
"confidence": 0.08649209635185473
},
{
"marker_id": 71,
"observed_center_px": [
1001.5,
551.75
],
"projected_center_px": [
1001.7471313476562,
552.2847290039062
],
"reprojection_error_px": 0.5890747071577294,
"confidence": 0.08751548667261201
}
]
},
"qa": {
"sanity_notes": []
}
}

BIN
test/homing/20260616_120456/cam0_debug.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 180 KiB

BIN
test/homing/20260616_120456/cam1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 76 KiB

3551
test/homing/20260616_120456/cam1_aruco_detection.json Normal file
View File

File diff suppressed because it is too large Load Diff

691
test/homing/20260616_120456/cam1_camera_pose.json Normal file
View File

@@ -0,0 +1,691 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-16T12:05:05Z",
"source": {
"detection_json": "/app/data/homing/20260616_120456/cam1_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam1",
"camera_matrix": [
[
1367.5723876953125,
0.0,
672.1165771484375
],
[
0.0,
1372.3011474609375,
445.8396911621094
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.01016925647854805,
0.7656787633895874,
-0.0031530377455055714,
-0.00288817984983325,
-2.490830183029175
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 35,
"used_marker_ids": [
69,
64,
58,
66,
103,
95,
97,
51,
54,
55,
75,
47,
77,
74,
85,
96,
52,
81,
62,
57,
105,
101,
71,
78,
82,
65,
80,
84,
99,
87,
70,
91,
90,
93,
104
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.01669716219734741,
0.0015530130263999152,
0.0008983437053381035,
0.0008983157544106726
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 1.7685482048099375,
"residual_median_px": 1.5050116514173932,
"residual_max_px": 3.68734500446118,
"sigma2_normalized": 8.826247430843121e-07
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
-0.8296902179718018,
0.04856207221746445,
-0.556107759475708
],
[
-0.20650699734687805,
0.8988339900970459,
0.3865906596183777
],
[
0.5186222195625305,
0.4355906546115875,
-0.7357253432273865
]
],
"translation_m": [
0.31146639585494995,
0.15941768884658813,
1.010162115097046
],
"rvec_rad": [
0.11326674166552995,
-2.4843106472248735,
-0.5896093173683601
]
},
"camera_in_world": {
"position_m": [
-0.23255103826522827,
-0.5984326601028442,
0.8547813296318054
],
"position_mm": [
-232.55104064941406,
-598.4326782226562,
854.7813110351562
],
"orientation_deg": {
"roll": 149.3720703125,
"pitch": -31.2398738861084,
"yaw": -166.0232696533203
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
5.108318008737832e-07,
-2.030876446668622e-07,
1.847398283306561e-07,
-9.7616015744967e-09,
9.054207137354404e-08,
1.6652401795418441e-07
],
[
-2.0308764466685943e-07,
1.5382020212494465e-06,
-6.297556683785447e-07,
-3.010072505572984e-07,
1.0689229244008563e-07,
-8.671432148962064e-07
],
[
1.8473982833065666e-07,
-6.297556683785541e-07,
2.588424752063583e-06,
-9.193339804491363e-08,
-1.7555965832587911e-07,
6.037665269982221e-08
],
[
-9.76160157449755e-09,
-3.0100725055729717e-07,
-9.193339804491597e-08,
1.1789721480957471e-07,
-1.8772945136575335e-08,
2.1678392569580849e-07
],
[
9.054207137354437e-08,
1.0689229244008568e-07,
-1.755596583258787e-07,
-1.877294513657538e-08,
7.498759148393252e-08,
-4.200084672079684e-08
],
[
1.6652401795418214e-07,
-8.671432148962051e-07,
6.037665269981562e-08,
2.1678392569580904e-07,
-4.200084672079688e-08,
9.640297784807404e-07
]
],
"parameter_std": {
"rvec_std_deg": [
0.0409507250203997,
0.07106067381441428,
0.092180785484839
],
"tvec_std_m": [
0.0003433616385235466,
0.0002738386230682818,
0.0009818501812805965
]
},
"camera_center_std_m": [
0.0010578573566047088,
0.0012453368073035017,
0.001463792458469902
],
"camera_center_std_mm": [
1.0578573566047087,
1.2453368073035016,
1.463792458469902
],
"orientation_std_deg": {
"roll": 0.10570612813240464,
"pitch": 0.06594269721083164,
"yaw": 0.053541286201001195
}
}
},
"observations": {
"markers": [
{
"marker_id": 69,
"observed_center_px": [
1132.0,
285.5
],
"projected_center_px": [
1134.9923095703125,
285.38287353515625
],
"reprojection_error_px": 2.9946010040321855,
"confidence": 0.8004430063137599
},
{
"marker_id": 64,
"observed_center_px": [
1160.25,
421.5
],
"projected_center_px": [
1163.9249267578125,
421.8023681640625
],
"reprojection_error_px": 3.68734500446118,
"confidence": 0.6780206236463221
},
{
"marker_id": 58,
"observed_center_px": [
1061.75,
363.25
],
"projected_center_px": [
1063.72900390625,
363.04876708984375
],
"reprojection_error_px": 1.9892086730865397,
"confidence": 0.6409044457372643
},
{
"marker_id": 66,
"observed_center_px": [
861.0,
135.75
],
"projected_center_px": [
860.8423461914062,
135.11590576171875
],
"reprojection_error_px": 0.6533989794800673,
"confidence": 0.5643748623006126
},
{
"marker_id": 103,
"observed_center_px": [
984.5,
388.75
],
"projected_center_px": [
985.591064453125,
388.5110778808594
],
"reprojection_error_px": 1.1169178214567097,
"confidence": 0.5680230430248981
},
{
"marker_id": 95,
"observed_center_px": [
888.0,
260.75
],
"projected_center_px": [
888.1265258789062,
260.22412109375
],
"reprojection_error_px": 0.5408857754384888,
"confidence": 0.5040987555384636
},
{
"marker_id": 97,
"observed_center_px": [
748.25,
129.5
],
"projected_center_px": [
747.3042602539062,
128.7545623779297
],
"reprojection_error_px": 1.204201360130154,
"confidence": 0.46133781313759503
},
{
"marker_id": 51,
"observed_center_px": [
906.75,
392.5
],
"projected_center_px": [
906.9967041015625,
392.4247131347656
],
"reprojection_error_px": 0.25793608860448214,
"confidence": 0.5161510750240749
},
{
"marker_id": 54,
"observed_center_px": [
707.0,
162.75
],
"projected_center_px": [
706.260498046875,
162.3862762451172
],
"reprojection_error_px": 0.824110495347403,
"confidence": 0.41077842645846463
},
{
"marker_id": 55,
"observed_center_px": [
773.75,
258.25
],
"projected_center_px": [
773.1185913085938,
257.3814392089844
],
"reprojection_error_px": 1.0738131975688514,
"confidence": 0.42449809621175133
},
{
"marker_id": 75,
"observed_center_px": [
1118.0,
858.25
],
"projected_center_px": [
1118.964599609375,
858.5298461914062
],
"reprojection_error_px": 1.0043735845047828,
"confidence": 0.32078570148601215
},
{
"marker_id": 47,
"observed_center_px": [
707.25,
218.0
],
"projected_center_px": [
706.2919921875,
217.7268829345703
],
"reprojection_error_px": 0.9961786487573198,
"confidence": 0.3835876006690484
},
{
"marker_id": 77,
"observed_center_px": [
1064.5,
835.0
],
"projected_center_px": [
1065.1744384765625,
835.8121337890625
],
"reprojection_error_px": 1.0556649800031064,
"confidence": 0.295743044190549
},
{
"marker_id": 74,
"observed_center_px": [
979.75,
758.0
],
"projected_center_px": [
979.26416015625,
758.2605590820312
],
"reprojection_error_px": 0.5512997270124411,
"confidence": 0.3359635861403907
},
{
"marker_id": 85,
"observed_center_px": [
546.0,
163.25
],
"projected_center_px": [
544.5795288085938,
163.747314453125
],
"reprojection_error_px": 1.5050116514173932,
"confidence": 0.3151234057766515
},
{
"marker_id": 96,
"observed_center_px": [
686.0,
331.5
],
"projected_center_px": [
685.0399780273438,
331.31890869140625
],
"reprojection_error_px": 0.9769525321278381,
"confidence": 0.3554108905483531
},
{
"marker_id": 52,
"observed_center_px": [
975.0,
818.25
],
"projected_center_px": [
974.9528198242188,
818.7924194335938
],
"reprojection_error_px": 0.5444674562606238,
"confidence": 0.28720673485083303
},
{
"marker_id": 81,
"observed_center_px": [
935.75,
766.5
],
"projected_center_px": [
935.2142333984375,
766.4912109375
],
"reprojection_error_px": 0.5358386874512324,
"confidence": 0.30145054927990633
},
{
"marker_id": 62,
"observed_center_px": [
652.5,
337.75
],
"projected_center_px": [
651.360107421875,
337.1482238769531
],
"reprojection_error_px": 1.2889878168290758,
"confidence": 0.3191799957107001
},
{
"marker_id": 57,
"observed_center_px": [
450.25,
90.5
],
"projected_center_px": [
449.19036865234375,
91.7260513305664
],
"reprojection_error_px": 1.6205000642145793,
"confidence": 0.26860824414385837
},
{
"marker_id": 105,
"observed_center_px": [
532.5,
214.5
],
"projected_center_px": [
531.2189331054688,
214.99610900878906
],
"reprojection_error_px": 1.3737745582393084,
"confidence": 0.28102521367518357
},
{
"marker_id": 101,
"observed_center_px": [
947.5,
889.75
],
"projected_center_px": [
946.0105590820312,
889.7289428710938
],
"reprojection_error_px": 1.4895897592281464,
"confidence": 0.25378363660843234
},
{
"marker_id": 71,
"observed_center_px": [
342.5,
164.75
],
"projected_center_px": [
342.4768981933594,
166.27276611328125
],
"reprojection_error_px": 1.522941341361428,
"confidence": 0.20828507797372528
},
{
"marker_id": 78,
"observed_center_px": [
275.25,
91.75
],
"projected_center_px": [
276.1173095703125,
93.466552734375
],
"reprojection_error_px": 1.923221043106055,
"confidence": 0.1903062602705479
},
{
"marker_id": 82,
"observed_center_px": [
845.0,
847.25
],
"projected_center_px": [
843.3731079101562,
846.445556640625
],
"reprojection_error_px": 1.8149123919458756,
"confidence": 0.2248605201960664
},
{
"marker_id": 65,
"observed_center_px": [
299.0,
144.0
],
"projected_center_px": [
299.8258361816406,
145.95309448242188
],
"reprojection_error_px": 2.120514903549027,
"confidence": 0.18160242191392414
},
{
"marker_id": 80,
"observed_center_px": [
248.0,
96.75
],
"projected_center_px": [
248.56906127929688,
98.8924331665039
],
"reprojection_error_px": 2.2167206888850366,
"confidence": 0.16263927873225353
},
{
"marker_id": 84,
"observed_center_px": [
674.75,
748.5
],
"projected_center_px": [
673.0452880859375,
746.7835083007812
],
"reprojection_error_px": 2.4191706147838157,
"confidence": 0.21110242529580486
},
{
"marker_id": 99,
"observed_center_px": [
184.75,
102.75
],
"projected_center_px": [
186.11749267578125,
105.2986068725586
],
"reprojection_error_px": 2.892305863695653,
"confidence": 0.14526599262838052
},
{
"marker_id": 87,
"observed_center_px": [
210.75,
180.75
],
"projected_center_px": [
212.3033447265625,
182.51739501953125
],
"reprojection_error_px": 2.352990649068414,
"confidence": 0.15014511583656154
},
{
"marker_id": 70,
"observed_center_px": [
526.25,
722.25
],
"projected_center_px": [
524.888671875,
720.3948974609375
],
"reprojection_error_px": 2.301004062219828,
"confidence": 0.15652476433958798
},
{
"marker_id": 91,
"observed_center_px": [
445.25,
709.25
],
"projected_center_px": [
443.6956787109375,
707.20751953125
],
"reprojection_error_px": 2.566640047778054,
"confidence": 0.147769899515473
},
{
"marker_id": 90,
"observed_center_px": [
498.5,
722.5
],
"projected_center_px": [
497.030517578125,
720.7365112304688
],
"reprojection_error_px": 2.2954893222279336,
"confidence": 0.1571012027138013
},
{
"marker_id": 93,
"observed_center_px": [
285.5,
521.25
],
"projected_center_px": [
286.38006591796875,
519.7478637695312
],
"reprojection_error_px": 1.7409564253182916,
"confidence": 0.1231418173996764
},
{
"marker_id": 104,
"observed_center_px": [
367.0,
617.5
],
"projected_center_px": [
366.587646484375,
615.7141723632812
],
"reprojection_error_px": 1.83281634920599,
"confidence": 0.14786630564511977
}
]
},
"qa": {
"sanity_notes": []
}
}

BIN
test/homing/20260616_120456/cam1_debug.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 214 KiB

BIN
test/homing/20260616_120456/cam2.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 240 KiB

4221
test/homing/20260616_120456/cam2_aruco_detection.json Normal file
View File

File diff suppressed because it is too large Load Diff

747
test/homing/20260616_120456/cam2_camera_pose.json Normal file
View File

@@ -0,0 +1,747 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-16T12:05:07Z",
"source": {
"detection_json": "/app/data/homing/20260616_120456/cam2_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam2",
"camera_matrix": [
[
1388.99072265625,
0.0,
933.082763671875
],
[
0.0,
1394.8729248046875,
562.4996948242188
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.019531700760126114,
-0.11213663965463638,
0.0026758278254419565,
0.0007694826927036047,
0.05339815095067024
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 39,
"used_marker_ids": [
73,
51,
82,
95,
66,
55,
86,
79,
97,
47,
54,
84,
96,
60,
72,
62,
53,
56,
67,
46,
85,
98,
68,
105,
50,
70,
90,
91,
76,
88,
104,
100,
65,
94,
80,
93,
87,
49,
99
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.010459464703921952,
0.000600512883123414,
0.0002694619125775263,
0.0002694585232559286
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 0.5308468666027378,
"residual_median_px": 0.442470638660156,
"residual_max_px": 1.0184954029835627,
"sigma2_normalized": 7.865855373327773e-08
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
-0.11123913526535034,
-0.9937126636505127,
-0.012687010690569878
],
[
-0.6987926959991455,
0.08728941529989243,
-0.7099784016609192
],
[
0.7066220045089722,
-0.07011179625988007,
-0.7041091322898865
]
],
"translation_m": [
0.06501124799251556,
0.4216889441013336,
0.7629748582839966
],
"rvec_rad": [
1.6611880808196222,
-1.8674322262849752,
0.7656557967967812
]
},
"camera_in_world": {
"position_m": [
-0.2372298538684845,
0.08128705620765686,
0.8374323844909668
],
"position_mm": [
-237.2298583984375,
81.28705596923828,
837.432373046875
],
"orientation_deg": {
"roll": -174.31349182128906,
"pitch": -44.96072769165039,
"yaw": -99.04487609863281
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
7.70808885712673e-08,
-5.051963537309151e-08,
6.275184896470588e-09,
8.219944671662197e-09,
3.0401303313197284e-08,
4.050671454678838e-08
],
[
-5.051963537309112e-08,
1.190708779865382e-07,
-7.710624609173114e-09,
-3.60891791637598e-08,
-3.822077534280391e-08,
-5.7320708312388815e-08
],
[
6.275184896470584e-09,
-7.710624609173487e-09,
3.4448589627302786e-07,
6.347555001744274e-08,
-3.73214863209708e-08,
-3.6939829357622016e-08
],
[
8.219944671662068e-09,
-3.6089179163759805e-08,
6.34755500174426e-08,
2.5430604878286124e-08,
2.514907754778194e-09,
7.848306963852704e-09
],
[
3.040130331319718e-08,
-3.8220775342803935e-08,
-3.732148632097092e-08,
2.514907754778194e-09,
2.366305405059915e-08,
3.099727891165752e-08
],
[
4.050671454678818e-08,
-5.732070831238883e-08,
-3.6939829357622505e-08,
7.848306963852644e-09,
3.0997278911657537e-08,
6.82205068714338e-08
]
],
"parameter_std": {
"rvec_std_deg": [
0.015907282309253958,
0.019770853151942665,
0.033628566543559106
],
"tvec_std_m": [
0.00015946976164240708,
0.00015382800151662619,
0.00026119055662759673
]
},
"camera_center_std_m": [
0.00030722285641275344,
0.0005444062391567311,
0.00035845833746427834
],
"camera_center_std_mm": [
0.30722285641275343,
0.544406239156731,
0.35845833746427835
],
"orientation_std_deg": {
"roll": 0.03168101724253478,
"pitch": 0.023469916191750515,
"yaw": 0.01918342103865859
}
}
},
"observations": {
"markers": [
{
"marker_id": 73,
"observed_center_px": [
492.5,
1044.0
],
"projected_center_px": [
492.4930114746094,
1043.94873046875
],
"reprojection_error_px": 0.051743640398894786,
"confidence": 0.2681332709796957
},
{
"marker_id": 51,
"observed_center_px": [
1265.25,
1035.5
],
"projected_center_px": [
1265.3367919921875,
1036.1767578125
],
"reprojection_error_px": 0.6823005105433091,
"confidence": 0.4130819814942474
},
{
"marker_id": 82,
"observed_center_px": [
547.0,
1041.75
],
"projected_center_px": [
547.1077880859375,
1041.7156982421875
],
"reprojection_error_px": 0.11311446441148582,
"confidence": 0.28570000902510134
},
{
"marker_id": 95,
"observed_center_px": [
1405.75,
993.0
],
"projected_center_px": [
1406.1728515625,
993.415771484375
],
"reprojection_error_px": 0.5930171760818419,
"confidence": 0.7223666558526649
},
{
"marker_id": 66,
"observed_center_px": [
1518.5,
947.0
],
"projected_center_px": [
1519.346923828125,
947.2127075195312
],
"reprojection_error_px": 0.8732264651916143,
"confidence": 0.7055288033501755
},
{
"marker_id": 55,
"observed_center_px": [
1343.5,
869.75
],
"projected_center_px": [
1343.1153564453125,
870.1083374023438
],
"reprojection_error_px": 0.5256960700643504,
"confidence": 0.5911102318971898
},
{
"marker_id": 86,
"observed_center_px": [
570.5,
855.75
],
"projected_center_px": [
570.8598022460938,
855.154541015625
],
"reprojection_error_px": 0.6957219691565115,
"confidence": 0.49213911198599
},
{
"marker_id": 79,
"observed_center_px": [
1192.75,
850.5
],
"projected_center_px": [
1192.2608642578125,
850.8606567382812
],
"reprojection_error_px": 0.6077228456730802,
"confidence": 0.5414756619284304
},
{
"marker_id": 97,
"observed_center_px": [
1462.0,
832.0
],
"projected_center_px": [
1462.5380859375,
832.4049072265625
],
"reprojection_error_px": 0.6734139427260099,
"confidence": 0.5279372649886516
},
{
"marker_id": 47,
"observed_center_px": [
1348.5,
797.75
],
"projected_center_px": [
1348.350341796875,
797.6093139648438
],
"reprojection_error_px": 0.20540238131674465,
"confidence": 0.5088850430842212
},
{
"marker_id": 54,
"observed_center_px": [
1406.0,
793.75
],
"projected_center_px": [
1406.117919921875,
793.9895629882812
],
"reprojection_error_px": 0.2670122344186656,
"confidence": 0.4782396466982758
},
{
"marker_id": 84,
"observed_center_px": [
620.0,
800.5
],
"projected_center_px": [
620.1146240234375,
800.3106689453125
],
"reprojection_error_px": 0.2213253600879973,
"confidence": 0.4467793485983816
},
{
"marker_id": 96,
"observed_center_px": [
1209.75,
783.0
],
"projected_center_px": [
1209.1610107421875,
782.8501586914062
],
"reprojection_error_px": 0.6077505767826198,
"confidence": 0.43618784694977364
},
{
"marker_id": 60,
"observed_center_px": [
588.25,
774.75
],
"projected_center_px": [
588.4884033203125,
774.8460693359375
],
"reprojection_error_px": 0.2570320222141527,
"confidence": 0.4149513143267376
},
{
"marker_id": 72,
"observed_center_px": [
705.5,
756.0
],
"projected_center_px": [
705.5302124023438,
756.4302978515625
],
"reprojection_error_px": 0.4313571957376901,
"confidence": 0.41472917138862403
},
{
"marker_id": 62,
"observed_center_px": [
1183.5,
747.5
],
"projected_center_px": [
1183.393798828125,
747.59423828125
],
"reprojection_error_px": 0.14198430392327666,
"confidence": 0.398140004533079
},
{
"marker_id": 53,
"observed_center_px": [
683.25,
712.75
],
"projected_center_px": [
683.28271484375,
713.240478515625
],
"reprojection_error_px": 0.4915683424421169,
"confidence": 0.3239734944910412
},
{
"marker_id": 56,
"observed_center_px": [
738.25,
696.0
],
"projected_center_px": [
737.8375244140625,
696.041748046875
],
"reprojection_error_px": 0.4145829330934417,
"confidence": 0.3443049703609756
},
{
"marker_id": 67,
"observed_center_px": [
616.0,
685.0
],
"projected_center_px": [
616.3634033203125,
684.5353393554688
],
"reprojection_error_px": 0.58989108129412,
"confidence": 0.30389163461125196
},
{
"marker_id": 46,
"observed_center_px": [
717.25,
662.5
],
"projected_center_px": [
716.9480590820312,
662.5203247070312
],
"reprojection_error_px": 0.30262420864781714,
"confidence": 0.3149845419556931
},
{
"marker_id": 85,
"observed_center_px": [
1316.75,
637.25
],
"projected_center_px": [
1316.73291015625,
636.42822265625
],
"reprojection_error_px": 0.8219550264218871,
"confidence": 0.32688682176810563
},
{
"marker_id": 98,
"observed_center_px": [
561.5,
643.0
],
"projected_center_px": [
561.7496948242188,
642.4434204101562
],
"reprojection_error_px": 0.6100232332233498,
"confidence": 0.29086841462927165
},
{
"marker_id": 68,
"observed_center_px": [
734.5,
628.0
],
"projected_center_px": [
734.5006103515625,
627.9244995117188
],
"reprojection_error_px": 0.07550295530465688,
"confidence": 0.29243937849297746
},
{
"marker_id": 105,
"observed_center_px": [
1256.0,
624.25
],
"projected_center_px": [
1255.47509765625,
623.591796875
],
"reprojection_error_px": 0.8418751832866965,
"confidence": 0.3029127688517622
},
{
"marker_id": 50,
"observed_center_px": [
685.75,
633.0
],
"projected_center_px": [
685.4552001953125,
632.6983032226562
],
"reprojection_error_px": 0.4218149716444313,
"confidence": 0.28667702620740154
},
{
"marker_id": 70,
"observed_center_px": [
581.5,
618.0
],
"projected_center_px": [
581.670166015625,
617.6889038085938
],
"reprojection_error_px": 0.3545945757920754,
"confidence": 0.2655177345943533
},
{
"marker_id": 90,
"observed_center_px": [
566.0,
585.0
],
"projected_center_px": [
566.2715454101562,
584.881103515625
],
"reprojection_error_px": 0.29643428238592895,
"confidence": 0.23219602732829617
},
{
"marker_id": 91,
"observed_center_px": [
559.5,
524.0
],
"projected_center_px": [
560.0989379882812,
523.4891967773438
],
"reprojection_error_px": 0.787176375460037,
"confidence": 0.21495162665262563
},
{
"marker_id": 76,
"observed_center_px": [
737.75,
537.0
],
"projected_center_px": [
737.5767211914062,
536.794677734375
],
"reprojection_error_px": 0.26866852861668133,
"confidence": 0.2109775341382568
},
{
"marker_id": 88,
"observed_center_px": [
576.25,
491.5
],
"projected_center_px": [
576.6868286132812,
491.4295654296875
],
"reprojection_error_px": 0.442470638660156,
"confidence": 0.18751626052772438
},
{
"marker_id": 104,
"observed_center_px": [
661.25,
443.0
],
"projected_center_px": [
661.183837890625,
443.0474853515625
],
"reprojection_error_px": 0.08143883183078994,
"confidence": 0.16576757504988693
},
{
"marker_id": 100,
"observed_center_px": [
730.75,
453.0
],
"projected_center_px": [
730.3933715820312,
452.9394836425781
],
"reprojection_error_px": 0.361726496152142,
"confidence": 0.16629537203475994
},
{
"marker_id": 65,
"observed_center_px": [
1209.0,
414.0
],
"projected_center_px": [
1208.3629150390625,
413.75823974609375
],
"reprojection_error_px": 0.6814141676114097,
"confidence": 0.1641051378333676
},
{
"marker_id": 94,
"observed_center_px": [
728.5,
406.0
],
"projected_center_px": [
728.5389404296875,
406.2892150878906
],
"reprojection_error_px": 0.291824817532418,
"confidence": 0.12091049607966889
},
{
"marker_id": 80,
"observed_center_px": [
1231.0,
373.75
],
"projected_center_px": [
1230.5965576171875,
373.7982482910156
],
"reprojection_error_px": 0.40631718378054876,
"confidence": 0.12350728340782749
},
{
"marker_id": 93,
"observed_center_px": [
757.5,
367.5
],
"projected_center_px": [
757.3904418945312,
368.3028869628906
],
"reprojection_error_px": 0.8103273743701577,
"confidence": 0.1305433400143518
},
{
"marker_id": 87,
"observed_center_px": [
1127.25,
335.0
],
"projected_center_px": [
1126.9609375,
334.5552673339844
],
"reprojection_error_px": 0.5304189600001262,
"confidence": 0.11698041029937327
},
{
"marker_id": 49,
"observed_center_px": [
742.0,
328.25
],
"projected_center_px": [
742.1221923828125,
329.2611389160156
],
"reprojection_error_px": 1.0184954029835627,
"confidence": 0.11651950992496915
},
{
"marker_id": 99,
"observed_center_px": [
1194.25,
321.0
],
"projected_center_px": [
1193.8394775390625,
320.6575012207031
],
"reprojection_error_px": 0.5346345525253963,
"confidence": 0.10558573070242767
}
]
},
"qa": {
"sanity_notes": []
}
}

BIN
test/homing/20260616_120456/cam2_debug.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 408 KiB

37
test/homing/20260616_120456/state_Arm1.json Normal file
View File

@@ -0,0 +1,37 @@
{
"status": "ok",
"link": "Arm1",
"joint": "y",
"joint_origin_world_mm": [
299.24409288922095,
101.1,
71.2
],
"joint_axis_world": [
-1.0,
0.0,
0.0
],
"mean_angle_deg": 42.11971851018807,
"circular_variance": 0.0,
"circular_std_deg": 0.0,
"num_pairs_used": 1,
"num_markers_matched": 2,
"per_pair": [
{
"marker_ids": [
197,
243
],
"skipped": false,
"angle_deg": 42.11971851018807,
"baseline_model_mm": 35.0,
"baseline_obs_mm": 32.87599167028967,
"weight": 1150.6597084601385
}
],
"accumulated_state": {
"x": 189.24409288922092,
"y": 42.11971851018807
}
}